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John A. Sorensen 
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SUMMARY 

Four nonlinear state estimator's were devised which pro- 
vide techniques for obtaining the angular orientation (atti- 
tude) of the aircraft. These techniques are alternatives to 
direct measurement by use of vertical and directional gyros. 
These estimators have the potential of being of low cost and 
of high reliability by implementation using solid state instru- 
ments and the microcomputer. 

An extensive FORTRAN computer program was developed to 
demonstrate and evaluate the estimators by using recorded 
flight test data. This program simulates the estimator 
operation, and it compares the state estimates with actual 
state measurements. 

The above program was used to evaluate the state esti- 
mators with data recorded on the NASA Ames CV-990 and Cessna 
402B aircraft From these evaluations, the preferred state 
estimator configuration was chosen. It was concluded that 
it is possible to estimate the aircraft attitude to the same 
degree of accuracy as is available by direct measurement . 

A preliminary assessment was made of the memory, word 
length, and timing requirements for implementing the selected 
state estimator on a typical microcomputer. 
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I 


INTRODUCTION 


Limited analytical studies have been made of using state 
estimation techniques coupled with low-cost sensors to replace 
the vertical and directional gyroscopes typically used on 
general aviation aircraft for flight control. These studies 
include previous work at Stanford University [1,2], an NRC 
Fellowship [3], and work at NASA Ames Research Center [4,5]. 

The objectives of this investigation were to 

(1) Combine the previous efforts to define estimator 

configurations which: (a) have the best overall 

features of the previous work, and (b) provide the 
basis for further analysis, design, and flight 
testing. 

(2) Specify the flight test data to be collected using 
the NASA Ames Cessna 402B aircraft . The data were 
to be used to test the estimator designs in the 
laboratory. 

(3) Code the candidate estimator designs on the IBM 360 
computer. Use the collected flight data to demon- 
strate the estimator performance under a variety of 
flight conditions and wind disturbances. Choose 
the estimator design yielding the best, performance. 

(4) Make recommendations regarding future system design 
and flight test efforts. 

This report is organized as follows. 

(1) Chapter II first presents details of previous esti- 
mator concepts which are suitable for determining 
the aircraft attitude without direct measurement. 
From these concepts, four nonlinear estimator con- 
figurations are designed. The method of implement- 
ing these equations digitally is explained. 

(2) Chapter III presents results of evaluating the 
candidate estimators with both Convair CV-990 and 
Cessna 402B data. From this evaluation, the best, 
estimator design is selected. 

(3) Chapter IV presents preliminary requirements for 
implementing the selected estimator software on a 
typical microcomputer. 
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(4) Chapter V summarizes the work, lists distinct con- 
clusions that have been made during the study, and 
makes recommendations for further research, testing, 
and development. 

C-5-) Appendix A. presents the details of- the- -computer 
program developed for processing the flight data. 

It serves as a user’s guide for additional flight 
data processing. 

(6) Appendix B presents software details of the selected 
state estimator. 

This report was prepared under Contract No. NAS2-9382 
for NASA Ames Research Center. The author wishes to 
acknowledge the ideas and technical exchanges provided during 
this study by Dr. Dallas G. Denery (who was technical monitor) 
and R.C. Wmgrove of NASA Ames Research Center. The author 
also wishes to express his appreciation to G.P. Callas, of 
NASA Ames, and W. Jolitz for their study support and personal 
interest . 
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II 


STATE ESTIMATOR SOFTWARE DESIGN 


When investigating ways in which an aircraft's angular 
orientation could be determined without direct measurement, it 
rapidly became apparent that the estimator could be based on 
a wide choice of equation formulations. There have been sever- 
al suggestions made [ 1 - 6 ] regarding how state estimators could 
be configured to obtain the aircraft attitude, and these are 
documented here. Then, composite methods which were chosen 
for implementation and testing with flight data are explained. 
Auxiliary software details are also presented. 

The equations used to design the state estimators are 
based on well known aircraft equations of motion [7] and on 
a knowledge of the combinations of state variables measured 
by different instruments other than attitude gyros. The 
measurements that may be used include linear acceleration, 
angular acceleration, magnetic field, altitude, airspeed, and 
control surface deflection. Altitude and airspeed can be com- 
puted from measurements of static and dynamic pressure, while 
rate gyros may be substituted for angular accelerometers. 

The estimators basically combine rapid measurements of angular 
acceleration (or rate) with independent (although noisy) com- 
putations of the attitude angles (based on measurements of 
magnetic field data and other variables). 


Previous Estimator Equations 

Prior to beginning this effort, there were three alternate 
suggestions [2,3,5] for the configuration of the state esti- 
mator equations. These configurations were analyzed, and they 
served as the starting point for the composite estimation 
methods mechanized m this study. They are documented here 
for later reference. 

Complementary filter approach [5] . Wmgrove suggested a 
method which makes use of the following aircraft kinematic 
equations [7] , 

h = f sm 8 - f cos 9 sm cp - f m cos cp cos 9 - g, 
c xm ym T zm 

9 = p + (q sm 9 +r cos cp) tan 9, 
a = q - pp + (g cos 8 cos <P+ f zm )/ v am > 
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P = -r+pa + (g cos 0 sin 9 +^)/^, 

♦wind - ~^zm S ^ n ’ (1) 

These equations use the assumptions that a and p are small^ 
angles. In these and subsequent- equations,' the standard air- 
craft dynamics equation notation is used [7] . That is, 

9 , 0 ,ij/ - roll, pitch, yaw angles, 

p,q,r - roll, pitch, yaw angular rates, 

a,p - angles of attack and sideslip, 

f ,f ,f - body-fixed linear accelerometer 

^ ym' 2m rea £ ings> 

V - measured true airspeed, and 

am 

\}/ . - yaw (or heading) angle of velocity 

win vector with respect to the airmass 

(wind). 

Other equations used which transform from wind to body 
axes include* 

0 = Y wmd + a cos 9 + P sin 9s 
Wd = ■ 

= ♦wind ~ P cos 9 + <* sin 9 . ( 2 ) 

Here, additional notation used is: 

Y wmd “ flight path angle with respect to the air- 
mass y 

h - altitude rate, 

ij/ - yaw (or heading) angle of the longitudinal 
m axis of the aircraft . 

In addition, the aerodynamic equations which represent measured 
angle-of-attack (a c ) and sideslip (P c ) are 
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a = a + mf / ( C Q S ) , 
c o znr v za m 

- k al + k a2 SF m + m W< C Z a«m S > ■ 

Pc “ mf ym/< C y(3 Q m S > ' 


Here, k 1 and k are constants, 8F is the flap angle, 

and C^p are aerodynamic coefficients, is the 

measured dynamic pressure, m is the aircraft mass, and S 
is the reference wing area. Use of Eqs. (3) assumes that 
stall conditions are not approached. 

It is assumed that measurements of haro-altitude h. , 

baro 

true airspeed V am , magnetic heading (or yaw) , linear 

acceleration components (f . f , f ), angular acceleration 
... xm ym zm 

components (p m , q^, r ffl ) , flap angle SF^, and dynamic pressure 

are available. Then, Eqs. (l)-(3) are combined into the 

four coupled nonlinear, fixed-gain complementary filters [8] 
shown m Fig. 1. In this figure, the hats ( A ) on variables 
indicate that they are estimated. Also, the notations s and 
c are used to represent sine and cosine, respectively. 

Note in Fig. 1 that an additional integrator is added to 
the front end of each channel. This is to remove the effect 
of possible biases which may exist in the linear and angular 
-‘celerometer measurements. 

Vector approach [2] . DeBra suggested that the attitude 
angles could be determined by measurement of the gravity and 
magnetic field vectors g and B. The differential equations 
to obtain these smoothed vectors are 


B = -(0 x B + K„(B - B) , 

B m 

g — —co x g + K g (-f m -g) , _ (4) 

where B is the three-component measurement from a three- 
m 

axis magnetometer, and f is the measured vector from a 

three-axis linear accelerometer package. The vector (o is 
the estimated attitude rate of the aircraft It could come 
from either integrating angular accelerometer outputs or direct 
(smoothed) rate gyro measurement. The quantities K g and K 
are appropriate gain vectors. s 
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h_ = ~f, m c<pc§ + f s6 - f c9scp- g 
"c zm ^ xm yin T 3 

= (cjs<p+?ccp)tan 9 

A A A A 

r>2 ~ pccp-ascp 


n 4 = pa+gcBsgi+f^ 

8 = sin”^(h/V arn } +accp+ psq> 

/V A A A A A 

* $ = ' |, WTn d -|3C9 + aS tP 


-PP + {gc0C9+ f 2m )/v am 


FIGURE 1.- COMPLEMENTARY FILTER FORM OF STATE ESTIMATOR [5] 
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With gravity and magnetic field vector estimates known, 
the direction cosine matrix which transfers from locally level 
to aircraft hody axes is 



Here, 33 ^ is the north component of B . 

xo ^ m 

From this matrix, the roll, pitch, and yaw angles can be 
determined. Only the magnetic field dip angle has to be 
occasionally updated when using this method so that the north 
component of the field B is kept current. The chief con- 

Av 

straints of this method are that* (a) linear accelerometer 
readings are only valid (for their use in Eq. (4)) during 
unaccelerated flight, and (b) the accuracy of the attitude rate 
estimate to must allow adequate tracking of the gravity vector 
for prolonged maneuvers and periods of accelerated flight. 

Kinematic filter approach [2,3], Sorensen and Tashker 
devised linearized, decoupled longitudinal and lateral filters 
which were based on estimating the perturbations of the air- 
craft state variables from their nominal values. They were 
termed "kinematic filters" because they took advantage of the 
kinematic equations which represent the measurements of linear 
and angular accelerometers. 

The matrix equation which represents the longitudinal 
kinematic filter is 
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Here, the quantities U Q and W q are the nominal values of 

longitudinal and normal airspeed. Also, 6 is the nominal 
pitch angle. ° 

The lateral kinematic filter is represented by 
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A(p m " A<p 
A ^m “ A ^. 


(7) 


In Eqs. (6) and (7), the "A" notation represents perturbations 
from the nominal. The subscript ”m" on input variables (p , 

V r m’ f xm’ f zm’ u m’ h m’ V V indlcates tha * these vari- 
ables are independently measured or computed. 

These equations must be expanded to include cross coupling 
terms, and they do not have to be restricted to small pertur- 
bations. Also, it was recognized that if an independent 
measurement of pitch angle 9^ were available, this could be 

used to replace the residual m airspeed (u - u) for the 

longitudinal equations. Thus, the resulting nonlinear kine- 
matic filter is represented by the schematic diagrams in 
Fig. 2.^ In this representation, it is assumed that the pitch 
angle 0 remains small. 


Note that the kinematic filter m Fig. 2 has the same 
complementary form as the filter suggested by Wingrove in 
Fig. 1 Also, note that the altitude h channel uses the 
longitudinal component of airspeed U as a separate input. 


8 




g 








Development of Composite Nonlinear Estimators 


For flight test investigation of the estimator concept, 
it is desirable to reduce the number of forms of estimators to 
one which has the best overall features. This is not possible 
without laboratory testing several configurations with actual 
data, this provided the motivation for this study. 

In examining the estimator concepts just discussed, some 
important points were developed: 

(1) It seemed advisable to retain the complementary 
filter form found in Figs. 1 and 2. This would enable 
combining the fast response angular accelerometer 
data (which would tend to drift) with stable, 
although noisy, independent measurements of the 
three attitude angles. 

(2) Because roll and heading angles could both be deter- 
mined from magnetometer data, it seemed reasonable 
to have separate channels for these quantities (as 
m Fig. 2 rather than Fig. 1). 

(3) To simplify the estimator, the sideslip angle (3 

(as in Fig. 1) was assumed to be zero. This feature 
could be added later, if required. 

(4) Two methods existed for computing independent measure- 
ments of the attitude angles (© , 8 . ) — the vector 

m m m 

approach discussed on page 5, and a method which com- 
putes roll and heading given pitch, which~is discussed 
later. It was not clear which of these methods was 
preferable. Thus, two forms of the composite esti- 
mators were first posed. 

As the study proceeded, two additional forms of the state 
estimators developed. These four methods are now explained. 

Method 1 . The basic form of the nonlinear estimator used 

m method 1 is shown in Fig. 3. This assumes inputs (p , q , 

m m 

r ) from three body-fixed orthogonal, angular accelerometers. 

The three attitude estimates are (cp, 9, $). The three indepen- 
dent computations of roll, pitch, and heading are denoted as 

<«v 9 c- V- 

The form of Fig. 3 was derived by combining and extending 
the design of Figs. 1 and 2. Note that if rate measurements 
(p^, q m , r ) were directly available, only three integrations, 

rather than six, would be required to implement this form. The 
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(q s $ + f c $) tan $ 



iF THE NONLINEAR ESTIMATOR 











decxsion of whether to use rate measurement devxces or angular 
accelerometers must be based on reliabxlity and cost consider- 
ations whxch were not a part of this study. 

Note in Fig, 3 that the nonlinear terms xn the feed- forward 
paths of each channel come from the dynamic equations whxch 
relate the.axrcraft body rates, (p., q,_ r) to the Euler angle 
rates ( 9 , 8 , i|r ) [7]. The nonlinear terms in the feedback paths 
for the pxtch ( 8 ) and headxng (\{f) channels relate the orxenta- 
tion of these Euler axes to the body axes representing pxtch 
rate (q) and yaw rate (r). 

If the angular accelerometer measurements are subject to 
bias, the bxas effects are removed (since they are observable) 
by expandxng each complementary filter to thxrd order, as xs 
illustrated xn Fig. 4 for the roll channel. 

To compute altitude rate h, and an estimate of flight 
path angle y, a fourth complementary fxlter xs added. Thxs 
is depicted xn Fxg. 5, and it xs the same as the altitude 



FIGURE 4.- MODIFICATION OF ROLL ESTIMATOR TO COMPENSATE 
FOR ANGULAR ACCELEROMETER BIAS 



h k 

baro 


FIGURE 5.- COMPLEMENTARY FILTER TO OBTAIN 
SMOOTHED ALTITUDE 
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channel m Fig 1. This filter complements barometric altitude 
h-baro Wltil a computation of vertical acceleration h c which 

is given m Eqs. (1). The estimated flight path angle and 
angle-of -attack are then computed as 

? = sin _1 (fi/V am ) , 

9 = (8 - 9)/cos 9 , (8) 

where V is the measured airspeed, 
am ^ 

In this method, the vector approach, discussed earlier, 
is used to compute independent measurements of ( q> , 9 , i|f ) . 

O v O 

The earth's magnetic field measurements are used directly. 
Estimates of the earth's gravity vector are obtained from the 
second of Eqs. (5). For this approach to work, digital logic 
must be used to cut out the accelerometer inputs to this equa- 
tion when it is sensed that the aircraft is turning or 
accelerating 

Assume that the aircraft rotates, in order, through Euler 
angles (\{/, 9, <p) from the north-oriented locally-level refer- 
ence frame to the aircraft fixed body frame. If c (with 

i , 3 = 1,2,3) are components of given by Eq. (5), then one 

can compute 


sm 

9 = 

c 

■ c 13 ’ 



sm 

^c = 

c 23 /cos 

8 e . 


sm 

v{/ = 

Y c 

c l 2 /cos 

9 c - 


cos 

t = 
Y c 

c^/cos 

0 . 
c 

(9) 


Equations (9) are then solved for the independent measurements 
of (<p c , e c , f c ). 

Method 2. In this method, the second channel in Fig. 3 
is changed to estimate angle-of-attack a rather than pitch 
angle $. The channel is replaced by the one shown in Fig 6, 
which also compensates for the pitch accelerometer bias. This 
is essentially the same as the third channel of the comple- 
mentary filter depicted m Fig. 1. 

The complementary measurement of the angle-of-attack comes 
from the first of Eqs. (3). In thi,s method, flap angle 5F m 

and dynamic pressure Q m measurements are required as well as 

knowledge of aircraft characteristics m, S, C% a , and cc q 
as a function of flap angle. 
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FIGURE 6.- MODIFICATION OF FILTER TO COMPUTE 

ANGLE-OF-ATTACK 


From the smoothed angle-of-attack, 
angle is computed to be 

0 c = a cos cp + sin~^(h/Y a;m ) 


the estimated pitch 

( 10 ) 


With the pitch angle 0 determined, the roll and' heading 

c 

angles cp and t|/ can be computed directly from the magnetic 
c c 

field measurements B . This has the advantage that the gravity 

m 

vector estimate g is not required, as in Method 1. 

The relationship between the aircraft-fixed components 

of B and the North-oriented, locally-level reference cora- 
m 

ponents (B Xq , O, B Zo ) are 



c0c\jc 

s<ps0cty-ccpsi}f 

ccps0c^+scpsv|/ 


c0si{j 

scps0s\j/+ccpcty 

ecps0sv|f-scpc\j/ 



Here, the notations s and c are used to represent sine 
and cosine, respectively. 

From Eqs . (11), the cosine of the heading angle is found 

to be 

cos = (B Xm + B Zq sin 6 c )/B Xo cos © c . (12) 
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Here, it is assumed that the local values of B x and Br, 

A o o 

are known and stored, or are computed by other means, as is 
discussed later. 

Also from Eq. (11), a quadratic expression can be found 
for the cosine of the roll angle 

+ B ym )o20 c! c \ - 2[< B Zo + B *m 3 V B z m o8 e] “'f’c 

+ [< B z 0 + B :% se c> 2 - B ?m c2f) cl = 0 • < 13 > 

With the two solutions (cos 9c^> cos 9 c 2 ^ f° r (13), the 

following expression is used to solve for two corresponding 
solutions of the sine of the roll angle 

sin q> Cl = B ym (c 2 9 Cl -l)/(B Zm c 9 C;L -B Zo ce c -B Xo se c c^ c ), 

i = l,2 (14) 

Then 

tan 9 C ^ = sin 9 c ^/cos 9 Cl , 1=1,2 . (15) 

Two ways are used to resolve the ambiguity m the solution 
for 9 c> In one case, a trial value 9 t is computed that is 

based on the previously computed values, <p 

9 t = 9 c _ x + p At . (16) 

Here, p is the estimate of the roll rate, and At is the 
sample period. Then, the solution to Eq. (15) is picked which 
is closer to Eq. (16). 

The other way to resolve the ambiguity is to assume that 
the aircraft usually makes coordinated turns. Then, the trial 
solution of roll angle is found from the coordinated turn 
relationship, 

tan 9 t = V a f/g , 

= Vqjq (q sin 9 +r cos cp)/g cos 8 ^ . (17) 
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Here, 13 the measured airspeed, and q, r, and <p are 

estimated values of pitch rate, yaw rate, and roll angle 
obtained from the estimator. Again, with this trial solution, 
the closer value of Eq. (15) is selected. 

F-inally, the equation 

sin = (S 9 0 -By m C 9 0 /B Zm )(B Zm /B Xo ) (18) 

is used to solve for \[/ . 

A potential problem exists with this method in that the 
solutions to Eq. (13) converge as ^ gets close to 0° or 180°. 
This is partially solved by use of Eqs. (16) or (17). However, 
Eq. (17) contains cp which could also be in error. Also, 
noise m p or c P c _ i m Eq. (16) could cause the wrong value 

to be selected. 

Method 3 . Although the techniques used to resolve the 

ambiguity problem for Method 2 worked for the data examined m 

this study, it was felt that certain data inputs could still 

produce incorrect solutions to the computed roll and heading Eqs. 

(12)-(18) . Thus, for Method 3, Eq. (17) was used directly to 

compute cp . Equation (10) was still used to compute 0 . 

c c 

Then, Eqs. (12) and (18) were used to compute . 

c 

One shortcoming of this method is that it relies on the 

coordinated turn assumption. The information contained in the 

magnetic data is not fully used. Also, as can be seen from 

Eq. (17), the computed roll angle tp is not independent of 

c 

the estimate cp. Flight tests under sideslip conditions are 
required to determine if this seriously degrades the per- 
formance of the estimator. 

Method 4 . Recall that the prime motivation for this 
research is to provide a low-cost alternative to direct measure- 
ment of the aircraft attitude angles. Thus, it is desirable to 
keep the estimators as simple as possible. 

The estimators developed m the previous three methods 
use a three-axis angular accelerometer package (or alternately, 
three rate gyros) to provide fast response attitude change 
measurements. But it is well known to pilots [9] that alti- 
tude, airspeed, yaw rate, and lateral acceleration measurements 
provide adequate information to fly straight-and-level . To 
these, heading angle measurements (from a compass) allow 
keeping a correct course. Thus, there was reason to believe 
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that a simpler configuration than that of the previously 
described three methods could he developed. 

A method, suggested by Denery [6] , makes use of a single 
yaw rate gyro measurement r . The equation for lateral 
acceleration is m 

ft + rXI - pW = a + g cos 8 sin 9 . (19) 

Here, a^ is the lateral acceleration due to aerodynamics, 
and (U,V,W) are body-fixed components of V„ . If V and pW 
are assumed to be negligible, Eq. (19) can be rewritten as 

sin <f a = (r m V am oos <« 0 -iy m >/g cos e o , (20) 

where fy is the measured lateral acceleration and V am is 
the measured total airspeed V_ . The pitch angle 9 again 

a C 

comes from Eq. (10). The angle-of-attack a is from Eqs. (3). 

c 

Equation (2) is solved for roll angle 9 . 

c 

The equation for normal acceleration is 

ft + pV - qU = a_ + g cos 9 cos 9 . (21) 

z 

If ft and pV are assumed to be negligible, Eq. (21) has the 
following solution for pitch rate: 

* -< f Zm + s cos 0 o COS ‘Po ) / V am 003 “ o • ( 22 > 

Here, 9 comes from Eq. (20). Then, the heading rate ft 
c c 

is computed to be 

ft c * (q c sm <P C + cos 9 c )/cos 9 C . (23) 

This equation is smoothed using the first-order complementary 
filter 

ft = ft c + ^(^-ft) » (24) 

where xjr comes from the magnetic field measurements and Eqs. 
(12) and (18). 


17 



In this method, most of the complementary filter structure 

• 

is removed. Only four integrators (three to compute £ as m 
Pig. 5 and one for Eq. (24)) are required instead of twelve. 

The shortcomings of Method 4 are the assumptions used to ob- 
tain Eqs. (20) and (22). 


Digitization and Auxiliary Software 

To mechanize the state estimators in digital form for 
flight testing, some additional software was required. Also, 
modifications of the continuous filter differential equations 
described previously to discrete form were required. These 
additions and modifications are explained here. 

Instrumentation corrections and computations .- Corrections 
and modifications must be made to the sampled signals used as 
inputs to the state estimators to remove known error effects. 

The linear accelerometers are subject to misalignment with 
respect to the aircraft reference body axis. Center-of-gravity 
(c.g.) offsets are also present which may require compensation. 
If (fx m> fy m ,f Zm ) are fk e sampled accelerometer readings, and 

^ < P a m’ ® a m , ^ a m^ r ®P p esent the small misalignment angles, then the 
corrected readings are 


f ' 

x m 

f 7m 


“'i'a. 


0 


m 


a m 


^ a m 

1 

-Tam 


-9. 




^ a m 

1 


fx 


m 


Ym 

L fs V 


(25) 


nlso, if (x am , y& m > z a m ) are the position coordinates of 

the accelerometer package with respect to the average aircraft 
c.g., then these accelerometer readings are further modified to 

f im = ~ V«« + Wa, + <« 2 + - P(qy am + , 

= - V% + Pm z a m + <p 2 + ? 2 )ya m - SCPXa* + , 

= f £n - Pm y *m + V^m + ^ m ' f <P x a m + ■ 

(26) 
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In Eq. (26), the acceleration terms (p , q m , r m ) come from the 

angular accelerometers, the rate terms (p, q, r) come from the 
estimator. 

In addition, the accelerometers are subject to biases 

(b _„„ , b_„ . b „„) and scale factor errors (e„„ . s , e ). 
axe' aye' aze v axe' aye' aze' 

If these terms are known, the readings are further corrected by 

the equations 


f =■ 

xm 

a 

+ 

e )i" 

axe' xm 

+ 

b axc ' 


f__ ° 
ym 

a 

+ 

e )i" 

aye' ym 

+ 

b ayc ’ 


f S3 

zm 

(i 

+ 

6 )±" 
aze' zm 

+ 

b azc * 

(27) 


The angular accelerometer and magnetometer are also sub- 
ject to misalignments, biases, and scale factor errors. If 
these errors are known, the sampled readings are also corrected 
by equations similar to Eqs. (25) and (27). 

If airspeed is derived from a pitot tube, the reading 
represents a component along the pitot tube axes. This 
reading V m must first be converted from indicated airspeed 

to true airspeed V , by the equation 


(28) 


altitude . 
of alti- 
m 


equation 
(29) 

Here, 

= airspeed filter gain, 

U = smoothed value of V , and 
n m 

At - sample period. 


V = V /TJp » V/Zo . 
m m r o nr 

Here, a is the density ratio which is a function of 
Computation of a is done from a table as a function 
tude by interpolation. Such a procedure is explained 
Appendix A. 

The airspeed measurement is then filtered by the 

U„ . - = U + k At(V' - U ) . 
n+1 n u ' m n' 
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The subscripts n and n+1 indicated current and predicted 
values (one sample period later). Then, if 0 is the rota- 
tion of the pitot tube up from the aircraft longitudinal axis, 
the components of the aircraft velocity are 


U=(-U n cos § + fi sin 0 v )/(sm 0 v sm 0 - cos 0 V cos 9), 

■ 

W-(-U n sm 0+h cos 0 y )/(sin 9^ sm 0 - cos 0 V cos 9), 

= j c os J t I sin 5 . (30) 

In Eqs. (30), the terms 0, h, and a come from the esti- 
mator. 

\ 

If the J-Tek Airspeed Sensor is used, total true airspeed 
V is sensed directly. Then, this is smoothed by the equa- 
tion 

^am n +i = ^am n + k u At(Vam - ^am n ) ■ (31) 

If dynamic pressure Q is not measured, it can be derived 
from the true airspeed by the equation 

6 = 0.5 pvf = 0.001189 oV* . (32) 

n r am am 

Again, the density ratio a is computed as a function of alti- 
tude fi . 

One further computation is required to determine the 
magnetic vector dip angle S 2 . This is used to compute the 

north and down components B and B found m Eqs. (11). 

If the magnetic field has unity magnitude, then these compon- 
ents have the values 


B xo = cos S 2 - 

B zo = Sln s 2 < 33 ) 


Eor a typical flight, the dip angle will be slowly varying 
as a function of aircraft geographical position. Thus, the dot 
product between the magnetic field vector B and the gravity 
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vector g would also vary slowly. This fact can be used to 
update Sg by the equation 


8 2n+l = S 2n + k S 

Here, k g is a slow gain, and B m and f m are 

magnetometer and linear accelerometer measurements of B and 
g> 


sin 


-1 


B 


ra 


m 


- 5 . 


~2n 

m m , 


At 


(34) 


Digitization . - Note that the nonlinear estimators 
depicted in Figs. 1-6 are in continuous (analog) form. To 
mechanize these filter equations on a digital computer required 
making some assumptions about the nature of the cross-coupling 
terms and feedback quantities. The assumptions for the roll 
estimator shown in Pig, 4 were: 

(1) The sample period is very small. 

(2) All trigonometric functions (sin cp, cos cp, tan 9) are 
constant over the sample period. 

(3) The feedback correction term (cp -cp) is constant over 
the sample period. 

The same type of assumptions were made for the altitude, yaw, 
and pitch (or angle-of-attack) filters. 

With these assumptions, the following expressions repre- 
sent examples of the discrete update equations used for the 
roll estimator: 

Boll accelerometer bias, b* 


r <p = <Pc n - 9 n , 

c bp = k Ocp r (p » 

6 Pn+l = Hn + c b p At • (35) 

Roll rate , p 

c 3 ^ ^ m n + k lcp r <p + k Pn ’ 

P n+ 1 = P n + c 3 At + c bp At 2 /2 . (36) 
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Roll angle , 9 



^n+l 


k n r , 

2 <p 9 1 

= 9 n + [P n + ^ n sin V n + *n cos $n )tan + c 4^ At 

y\ ^ ^ 

+ [°3 + ( c 5 sm <P n + c? cos 9 n )tan e Q ]At /2 
+ [ c b p + (°bq sm 9 n + c br cos 9 n )tan § Q ]At 3 / 6 . 


(37) 


In Eqs. (35)-(37), the subscript n+1 again indicates the 
updated value projected one sample period At into the future. 
The subscript n represents the current estimated values. 

Also, in Eq. (37), the quantities c 5 and c 7 represent terms 


similar to c 3 (Eq. (36)) from the pitch and roll channels, 
respectively. The terms c b( ^ and c br represent terms similar 
to c b (Eq. (35)), also from the pitch and roll channels. 

Xr 

Further details of these expressions and for the other channels 
are found in Appendices A and B. 


With fast sampling rates (five or more samples per second), 
the direct integration implied by the Eqs. (35)-(37) produces 
adequate accuracy. If the sample rate were to decrease, modi- 
fications would be necessary to account for these effects. 

Such methods for implementation of sampled data systems such 
as Tustm’s method and the hold equivalence [10] have been 
developed to provide discrete transfer functions with the same 
characteristics as the continuous system. Investigation of 
these procedures was beyond the scope of this effort. 


Gain selection .- The gams for the filters shown m Fig. 

3 are found by assuming that the coupling terms between the 
roll, pitch, and heading equations are zero, this decouples them 
into three linear second-order filters. They have character- 
istic equations 


s* 


+ 2t CD s + 

3 n 


“n = 0 


(38) 


For these filters, the error characteristics of the 
accelerometer inputs (p , q m , ^ m ) and the angles computed from 

magnetometer data ( 9 m , © m , ty m ) were unknown. To obtain some 
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appropriate gams with which to begin to test the estimators 
with flight data, it was assumed that the input errors could 
be modeled as white noise with Gaussian distribution. Then, 
Kalman filter theory [11] was used to compute the gains. 

With this assumption, the gams for the roll filter of 
Fig. 3, for example, are 

k- = to = a • /a , 

1 n P' <P m J 

k 2 = 2 £ % ~ 1-414 /kj . (39) 


Here, is the assumed standard deviation of the roll 

accelerometer measurement noise. Also, is the assumed 

standard deviation of the independent roll angle computation 
noise. Similar methods are used to compute gams k^-kg. 

If bias terms are added to the estimator (such as m Fig. 
4), the filter equations are observable but not disturbable 
[12]. Thus, the Kalman f lit ei 4 ' theory cannot be used directly. 
To make this adjustment, the filters* characteristic equations 
were expanded to the assumed form 

(s 2 + 2£ 0> n S +CZ5^)(S +(o n > = 0 . (40) 


Then, the resulting gains become 



9 


k x = (1 + 2?) J , 


k 2 = (1 + 2?) co n . (41) 

2 

Again, o> n was taken to be cr p/ cr <p m ’ and damping term 

? was /2/2. Similar methods were used for gam selection m 
the pitch (or angle-of-attack) , yaw, and altitude filters. 
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III 


STATE ESTIMATOR PERFORMANCE EVALUATION 


After developing the candidate state estimator configura- 
tions, the next step was to analyze them using recorded flight 
test data. The purposes of this step were to: 

(1) Demonstrate that the estimators actually worked as 
predicted using real (rather than simulated) input 
data. This is a step closer to actual flight test. 

(2) Compare the performance of the estimation methods so 
that a final configuration could he chosen. 

(3) Note the limitations of the estimators m severe wind 
conditions, unusual aircraft attitudes, and the 
presence of typical instrument errors. 

The details of the evaluation are now discussed. 


Evaluation Procedure 

To enable evaluating the estimator methods, a FORTRAN 
digital computer program was developed for the NASA Ames IBM 
360/67 which simulated operation of the digital estimator 
methods described in Chapter II. A complete description of 
this program, its inputs, its output, and its various capabili- 
ties are documented m the form of a user's guide in Appendix 
A. 


The program is set up to read in sequentially recorded, 
sampled, digital flight data. These data act as the driver for 
the program. The program makes necessary pre-estimation compu- 
tations and then simulates the operation of the digital soft- 
ware as it operates in a sequential fashion. After the data 
are read m, the steps executed are* 

(1) All sensor data not actually present are artificially 
generated. For example, rate gyro data are smoothed 
and then differentiated to produce artificial" angular 
accelerometer data. 

(2) Artificial errors are optionally added to the data 
to allow determining the resulting effect on esti- 
mate accuracies. By varying the error magnitudes, 
the program user can obtain performance sensitivity 
d.ata These sensitivity data are useful for specify- 
ing sensor accuracy requirements. 


25 



(3) The data are filtered and modified with correction 
terms to remove known sensor errors. For example, 
obvious biases are removed from the linear acceler- 
ometer readings. This is the first step in the simu 
lated estimator software. 

(4‘) The independent calculations of roll, pitch, and 

heading (<p , 9 , ) angles are made. An option 

c c c 

flag determines which computation method described 
m Chapter II is used. 

(5) The primary filter equations are executed to produce 
estimates of attitude angles and rates (cp, @, $, p, A 
q, r), angle-of-attack a, and flight path angle y 
Also, smoothed values of altitude h and true air- 
speed V a are generated. 

(6) The results are compared to directly-measured values 
of the state variables. For example, the estimated 
attitude angles are compared to angles measured by 
an LTN-51 inertial navigation system. Comparison 
consists of computing the means and standard devia- 
tions between the estimated (or smoothed) and direct 
ly measured data. 

(7) The results are recorded either m numerical or plot 
form. 

When the program coding and debugging was completed, anti 
cipated data from the Cessna 402 aircraft were not yet avail- 
able. Therefore, data from a Convair CV-990 flight were used 
to check out the program. These data did not contain magnet- 
ometer measurements, so these quantities were artificially 
generated from the INS measurements. 

The CV-990 data were divided into three segments that 
tested the estimator configurations under longitudinal motion, 
lateral motion, and both modes together. Details of these 
data are described later. 

The four estimator methods described in Chapter II were 
each tested with the three segments of CV-990 data. From 
these runs, it was shown that each of the methods works to 
varying degrees of accuracy with actual data. Thus, it was 
further concluded that the concept of estimating attitude 
angles, rather than direct measurement, is valid. 

Towards the end of this study, a small amount of data 
(SO sec) taken from the Cessna 402 aircraft became available. 
This provided additional information because the data included 
actual magnetometer measurements. The estimators were tested 
with this data, and further conclusions were made. 
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Test results from using the CV-990 and C-402 data are now 
discussed. 


CV-990 Performance Results 


The Ames CV-990 instrumentation and associated measure- 
ments included: 


Inertial navigation system 
Directional gyro 

Three-axis linear accelerometer - 
Three-axis rate gyros 
Baro-altimeter 
Air data system 


cp, 0 not available) 
* 

**■ V f Z 

Pi q» r 
°baro 


Simulated angular accelerometer data were obtained by differ- 
entiating the rate gyro data. Simulated magnetometer data 
were obtained using the equations 



c0c\|/ 

scps0cil/-c<psi{/ 

ccps0ci(/+scpsi|f 


C0sty 

scps0s^+c<pc^ 

ccps0s\J/-scpc\|/ 


-s0 

scpc0 

ccpcG 


B 


xo 

0 


B. 


3 o- 


(42) 


Here, the values of B Xq and B Zo were taken to be those 

typical of the San Francisco Bay area (dip angle of 62°). The 
angles (<p, 9, 'jO were taken from the INS and directional gyro 
measurements . 


The recorded data at each sample point (approximately 
1.018 sec apart) consisted of an average of the 20 previous 
samples taken approximately every 0.05 sec. Thus, the data 
had some built-m smoothing and some inherent lag. No attempt 
was made to smooth the data further. Misalignments between 
the instrument axes, acceleration measurement effects due to 
displacement from the aircraft c.g., and other instrument 
errors were unknown. It was found that to obtain acceptable 
results, the bias and scale factor corrections which appear m 
Table 1 had to be made to the linear accelerometer measure- 
ments. 
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TABLE 1.- CORRECTIONS TO CV-990 LINEAR ACCELEROMETER 

MEASUREMENTS 


ACCELEROMETER 

BIAS - FT/SEC 2 

SCALE FACTOR ERROR 

f x ■ 

~ 2.39 

-0.4098 

f y 

0.21 

— 


-3.65 

— - 


The three CV-990 flight sequences used to test the esti- 
mators were* 

(1) 400 sec of level flight, simulated approach (down to 
90 ft altitude), and climbout , primarily longitudinal 
motion. 

(2) 150 sec of level coordinated turn of 180°; primarily 
lateral motion. 

(3) 250 sec of level flight, turn of 30°, simulated 
approach, and climbout, combined longitudinal and 
lateral motion. 

Methods 1 and 2 were first tested using the above data 
sequences. The performance was assessed, as mentioned before, 
by examining the statistical differences between the estimated 
attitude angles and those directly measured. There are obvious 
discrepancies m these data besides the normal electronic noise, 
'’’here would exist some misalignment between the measured atti- 
tude axes (X and Y of the LTN 51), the directional gyro 
(Z axis), the rate gyro axes, and the linear accelerometer 
axes. The accelerometer would sense all rate terms by not 
being located on the aircraft center-of -gravity. The gyros 
would have acceleration dependent terms and as mentioned above, 
the accelerometers had large bias and scale factor errors. 

To make the comparisons, Methods 1 and 2 were run using 
the first two data sequences. For gain selection, it was 
assumed that the input measurement noise had the following 
standard deviations* 

2 

Angular accelerometers " “ 0.002 rad/sec 

Independent angle computations - ^cp m > o 0 m > 0 \Jf m - rad. 
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o 

Linear accelerometers - off , a* , cf -0.03 ft /sec 

•*-x iy ■‘•z ' 

Altimeter - - 10 ft. 


With these values, the gains were computed using Eqs. (41). 

They appear in Table 2. In addition, for Method 2, the follow- 
ing constants were used in the process of computing the angle- 
of-attack a m [5] (see Eq. (3)): 


m = 5652.2 slugs 

Cz Q * 5.15662 - 

S = 2250 ft 2 

k i =» 0.09 rad 
a l 

k a2 = -0.13674 - 


TABLE 2. FILTER GAINS FOR INITIAL TESTS OF 

METHODS 1 AND 2 



BIAS GAIN kg 

RATE GAIN kj 

POSITION GAIN k 2 

Altitude filter 

0.03 

0.2330 

0.7493 

(ft. fi> 




Attitude filters 

0.03162 

0.2414 

0.7634 

. A A A A A A. 

(p,cp,q s e,r,\l/) 





For each run, the mean and standard deviation between the 
estimated attitude angles and those measured by the INS and 
directional gyro were computed. A comparison of these statis- 
tical quantities appears in Table 3. 

For Sequence No. 1, the performance of both methods is 
acceptable, although the standard deviations in (cp, 8, \J/) are 
30%-60% better with Method 2. The performances of both methods 
could be improved with gain adjustment . 

For Sequence No. 2, Method No. 2 is substantially better 
than Method 1. The estimation algorithm m Method 1 is set so 
that whenever the angular rate magnitude becomes greater than 
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TABLE 3.- COMPARISON OF METHODS 1 AND 2 FOR THE FIRST TWO 

CV-990 DATA SEQUENCES 


SEQUENCE 

ANGLE, DEG 

METHOD 

1 

METHOD 2 

11 

a 

m 

a 

1 

<p 

-0.52 

1.38 

-0.21 

0.54 


9 

-0.86 

1.66 

0.42 

0.98 


¥ 

-1.13 

2.75 

0.82 

2.03 

2 

9 

0.63 

4.15 

0.24 

2.14 


e 

4.22 

2.20 

-0.93 

0.54 


♦ 

-7.24 

3.72 

0.56 

3.98 


a fixed limit, the pitch and roll angles are updated open-loop 
by use of simulated (integrated) angular accelerometer infor- 
mation. For the gyro data which was used to generate this 
information, the noise and drift rates were probably too great 
(i.e., the measured angular rates didn't match the measured 
changes in INS angles) to allow good angular tracking, even 
for only a two-minute span. Thus, Method 1, m its present 
form, was judged not adequate for lateral transient tracking 
with the given quality of rate information available on the 
CV-990 flight. 

There are probably modifications to Method 1 which would 
improve performance. For example, the acceleration vector 
used to track gravity could be modified to account for expected 
reorientation caused by a turn. The estimator performance 
could be improved with a better knowledge of the instrumenta- 
tion errors. It also would be desirable to obtain more data 
sequences and work with the raw data at each sample point 
rather than data averaged over 20 samples. 

At this point in the study, Methods 3 and 4 (discussed m 
Chapter II) were introduced. Again, the motivation for Method 
3 was to remove the ambiguity of the equations for determining 
roll angle which exists in Method 2. The motivation for 
Method 4 was to test the overall estimation concept using a 
simpler set of instruments. 

The two new estimator concepts were tested with the same 
data sequences described previously. Comparisons of the 
resulting means and standard deviations of the difference 
between the measured and estimated roll, pitch, and beading 
angles for Methods 2, 3, and 4 are presented m Table 4. 
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TABLE. 4.- MEANS (m) AND STANDARD DEVIATIONS (a) OF ESTIMATOR 
STATE ERRORS FOR THREE MECHANIZATIONS DURING THREE 

FLIGHT SEQUENCES 


METHOD 

2 3 4 

SEQUENCE ANGLE, DEG 




m 

a 

m 

a 

m 

a 

1 

9 

-0.21 

0.54 

-0.40 

1.31 

-0.51 

1.23 


0 

0.42 

0.98 

0.42 

0.95 

0.40 

0.98 


+ 

0.82 

2.03 

0.94 

1.48 

0.97 

1.51 

2 

9 

0.24 

2.14 

0.70 

2.09 

0.45 

1.87 


6 

-0.93 

0.54 

-0.91 

0.74 

-0.91 

0.32 


♦ 

0.56 

3.98 

-0.D7 

2.29 

0.57 

1.98 

3 

9 

0.19 

0.96 

0.31 

5.14 

0.18 

2.16 


e 

-0.84 

2.24 

-0.82 

2.21 

-0.88 

2.05 


* 

-1.48 

4.94 

-1.19 

3.82 

-1.49 

3.37 


The values shown in Table 4 only relate relative per- 
formance for the given set of filter gains and instrument 
calibration factors chosen for the particular runs. General 
improvements can be made by parameter adjustment as is dis- 
cussed next. The Table 4 data show that not relying on the 

(simulated) magnetic field for computing the angle <p (Methods 

3 and 4) resulted in an improvement in the standard deviation of 
the heading angle difference vjf. The pitch angle was generally 
unaffected by the estimator method used except during Sequence 
2. Here, removing the simulated pitch angular accelerometer 
data (in Method 4)^lowered the standard deviation of the pitch 
angle difference 0. The mean and standard deviations of the 
roll angle difference <p generally increased due to the 
assumptions of Methods 3 and 4, as would be expected. 

A limited study was made to determine what improvements 
could be made to the performance by changing the model para- 
meters used to compute a Q as a function of flap angle and 

the complementary filter gains. Recall that Methods 2-4 use 
the computed angle-of-attack of Eq. (3). In this expression, 
f z is the adjusted aircraft-fixed, downward component of 

measured acceleration equal to 

f z m = (1 + s ag; C )fz m + b a zc • < 43 ) 
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Here, s and b are the scale factor and bias calibra- 

’ azc azc 

tion terras. Thus, Eq. (3) has the parameters k - k „, 

ttl j (X £ 

e b and (m/Cg S) which can all be adjusted to affect 

aZC cLZC U 

the ..computed a_ - and 0.. For the data listed previously, 

c c 

the values of k^ and were 0.09 and -0113674, respec- 

tively. A test was made using data input Sequence 1 with 
Method 2 and the values (k - =-0.9; k 2 = -0.095). This pro- 
duced the results: a 


ANGLE, DEG 

m 

CT 

<P 

0.11 

0.64 

0 

-0.22 

0.60 

* 

-0.39 

1.28 


By comparing these data with the results in Table 4, it can 
be seen that the mean errors were cut in half, and general 
improvement was realized in the standard deviations of 9 and 
\}/. This indicates the importance of having a good knowledge of 
the aircraft model for a Q in Eq. (3). It is expected that 

additional improvement could be made by adjustment of the 
other instrument calibration quantities. 

The previous data were produced based on the assumptions 
that the angular accelerometer measurements had noise with 

o 

standard deviations of 0.002 rad/sec . It was also assumed 
that the angles measured from the magnetometers and by use of 
Eq. (3) had noise with standard deviations of 0.02 rad. The 
resulting Kalman gains produced filters with natural frequency 
of 0.3162 rad/sec. Another test was made m which this fre- 
quency was changed to 0.2 rad/sec by gain adjustment. The 
results were (again with Method 2 and Sequence 1) * 


ANGLE, DEG 

m 

<j 

<P 

i 

0.10 

0.44 

0 

-0.22 

0.57 

* 

-0.39 

1.09 


Here, it is seen that each standard deviation is decreased 
somewhat compared to the previous data. Again, further improve 
ment can be made by further gam adjustment. 


32 



and 


For further comparison, the changes m k k 0 , 

(Z X CC A 

the filter gains were used also to regenerate results for 
Sequence 1 using Methods 3 and 4. The results are compared in 
Table 5 with those of Table 4. As can be seen m Table 5, 
these changes almost uniformly lowered all the mean differences 
and the associated standard deviations. 


TABLE 5.- COMPARISON OF MEANS (m) AND STANDARD DEVIATIONS 
(a) OF ESTIMATOR STATE ERRORS FOR SEQUENCE 1 WITH 
ORIGINAL AND MODIFIED FILTER PARAMETERS AND CONSTANTS 
k x AND k 2 (FLAP ANGLE EFFECT) 


PARAMETERS 

ANGLE, 

DEG 

2 

m 

a 

Original 

<P 

-0.21 

0.54 


0 

0.42 

0.98 


f 

0.82 

2.03 

Modified 

<P 

0.10 

0.44 


0 

-0.22 

0.57 


* 

-0.39 

1.09 


METHOD 


3 4 


m 

CT 

m 

a 

-0.40 

1.31 

-0.51 

1.23 

0.42 

0.95 

0.40 

0.98 

0.94 

1.48 

0.97 

1.51 

-0.36 

2.06 

-0.55 

1.06 

-0.21 

0.57 

-0.23 

0.66 

0 

1.07 

-0.03 

0.93 


From Tables 4 and 5, the following conclusions can also be 

made: 

(1) The pitch angle accuracy is essentially independent 
of the estimator method used. Thus, for this data, 
no value is gained from use of the pitch angular 
accelerometer, as m Methods 2 and 3 

(2) The roll angle estimates 9 are generally more 
accurate in Method 2 m which^the magnetic field is 
used to smooth both 9 and $. This is expected 
because the aircraft does not always obey the coordin- 
ated turn assumption which is inherent in Methods 3 
and 4. 

(3) The heading angle estimates $ are closer to the 
directional gyro measured values when the roll angle 

estimate 9 is assumed to be a function of $ 
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(Methods 3 and 4). This is presumed to be due to an 
inherent disagreement between the INS and the direc- 
tional gyro measurements. 

(4) Method 4 produces essentially equivalent accuracy to 
Method 3. Thus, Method 4 is pref.err.ed to Method 3 
for this "data because one additional rate sensor 
(the roll angular accelerometer) can be omitted. 

Plots comparing the estimated and measured roll, pitch, and 
heading angles using Method 2 on data Sequence No. 1 are shown 
m Fig 7. The agreement is good despite the relatively "noisy" 
aircraft trajectory. It can be concluded that the nonlinear 
estimators work well using actual flight data Based on the 
limited data trials, it appears that they provide angle esti- 
mates that are adequate for flight control. 



I 1 L 
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TIME IN SECONDS 

FIGURE 7.- METHOD 2 ESTIMATED ORIENTATION ANGLES FOR 

DATA SEQUENCE NO 1 
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Cessna 402B Performance Results 


Towards the end of this study, a small amount of data (80 
sec period) became available which had been collected on the 
Ames Cessna 402B aircraft. These data were collected during 
the final approach and landing portion of a May 1977 flight at 
Crows Landing, California. This portion of the flight was 
reported to be subject to high lateral winds such that notice- 
able sideslip conditions prevailed. However, the INS measure- 
ments of wind magnitude and direction were not functional for 
this flight. Furthermore, the collected data had a high con- 
tent of spikes and data dropouts. These anomalies were re- 
moved by interpolation and other manual techniques to make the 
data usable. 

It was highly desirable to use these data for further 
estimator investigation because actual magnetometer data were 
recorded. Furthermore, all three attitude angles were measured 
and available from the INS system. (Recall that for the CV-990 
data, heading angle was taken from a directional gyro, and 
magnetometer measurements were artificially generated.) Thus, 
these new data could produce new insights. 

A serious problem with the 402 data was the high error 
content of the baro-altimeter recording. The baro-altimeter 
is a key instrument for computing pitch angle m Methods 2-4, 
Thus, it was decided to modify the program so that pitch angle 
wasn't estimated, but instead it was read directly from the 
INS. Thus, in the subsequent tests, only the roll and heading 
angles were estimated. However, this was still significant 
because it represented using actual magnetometer data to deter- 
mine both roll and heading angles. 

The instrumentation that is available on the Ames 402B is 
listed m Table 6. Also listed is the output range of the 
instruments, the number of digits recorded, and the equivalent 
accuracy. Details of this flight data system and its cali- 
bration can be found in Ref. 13. Again, as with the CV-990 
system, the relative alignments of the three-axis instrument 
packages were unknown. Also, the locations of the linear 
accelerometers with respect to the aircraft center of gravity 
were unknown. 

For the 402B flight, the sample period was 0.0702249 sec 
which represented a rate of about 14 samples per second. Ini- 
tially, no instrument correction terms were included in the 
data processing. Gams were held the same as those used for 
generating Table 5. 
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TABLE 6.- INSTRUMENTATION CHARACTERISTICS OF AMES 

CESSNA 402B [13] 


INSTRUMENT 

UNITS 

FULL SCALE 
READING 

BITS 

EQUIVALENT GRANULARITY 

Rate gyros 
(p,q 9 r) 

deg/sec 

+15 

10 

0.029°/sec 

INS angles 
(cp,e,i{/) 

deg 

+180 

14 

0.022° 

Linear 

Accelerometers 

<W f z> 

g 

+0.5 (f x ,fy) 
±3.0 (f z ) 

10 

0.0040g =0.03 ft/sec 
0.0059g = 0.19 ft/sec 

Magnetometers 

(B ,B ,B ) 

' x’ y 5 z' 

Gauss 

+600 

10 

1.17 Gauss 

Altimeter 

^ h baro^ 

ft 

-1000 

+9000 

10 

9.78 ft 

Airspeed 

< V a> 

kts 

250 

10 

0.244 kt = 0.41 ft/sec 

Control 
Surfaces 
(SF, etc.) 

deg 

Full surface 
deflection 
(0°-45°) 

10 

0.044° 


Figure 8 shows the initially estimated and INS measured 
roll and yaw angles for the 80 sec segment of the 402B flight 
using Method 2. As can be seen for this run, the estimated 
angles follow the same trends as the INS measured angles. How- 
ever, there is a large bias m the estimated roll angle. Also, 
the estimated heading angle increasingly deviates from the 
measured value as the angle becomes closer to 0° . For this ini- 
tial test, the mean differences and associated standard devia- 
tions between estimated and measured roll and heading angles were* 

ANGLE, DEG m a 

cp 6.39 1.82 

tjr 3.54 8.43 
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FIGURE 8.- INITIAL ESTIMATED AND MEASURED ROLL AND HEADING 

ANGLES FROM CESSNA 402B DATA 


It was concluded that signals of the three-axis magnetometer 
(which is mounted in the vertical stabilizer of the 402B) were 
being distorted by the aircraft structure, and that corrections 
should be made. 

The magnetometer data were examined at discrete points 
along the trajectory and compared to the corresponding INS 
measurements. It was determined that the characteristics of 
the distortions were such that the signals were subject to both 
misalignment and scale factor errors. Some calibration calcu- 
lations were made using discrete points of the data, and the 
resulting preliminary error magnitudes were determined to be: 

Misalignment (\|/g, 9g, cpg) =1.9°, 1.2°, 6.6° , 

Scale factor error ( £ cbx > e cby > s cbz ) = -0.15, 0, -0.05. 
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Corresponding correction terms were placed m the esti- 
mator software to remove these errors. The data were repro- 
cessed (again using Method 2) and the results are shown m 
Tig. 9. The resulting means and standard deviations were 

ANGLE, DEG m or 


cp 0.005 1.58 

^ -0.336 1.38 


As can be seen, this is a substantial improvement. A closer 
match could be made between the estimated and INS measured 
angles by further adjustment of gams and error correction 
terms. It again can be concluded that this method of estimat- 
ing attitude angles potentially works extremely well. The 






FIGURE 9.- ESTIMATED AND MEASURED ROLL AND HEADING ANGLES 
WITH MAGNETOMETER DATA CORRECTIONS 
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method is tolerant to typical instrument misalignment errors. 

A reservation is that these results were obtained by assuming 
that pitch angle was known exactly. 

The same data sequence was processed using estimator 
Methods 3 and 4. A comparison of the statistical results 
appears in Table 7. As can be seen from these results. Method 

2 performs significantly better than Method 3. In turn, Method 

3 performs significantly better than Method 4. The performance 
obtained from Method 4 is unacceptable. Thus, it is concluded 
that: 

(1) In 'this flight sequence, where frequent attitude 
transients and sideslip conditions prevail, the 
coordinated turn assumption is continually violated. 
Thus, inherent errors are present m Methods 3 and 
4. Significant information is obtained from the 
magnetometer data in Method 2 for directly computing 
the roll angle. 

(2) In the presence of consistent attitude transients, 
the estimated roll rate (p) information (Methods 2 
and 3) becomes more important. The equations used 
to derive Method 4 include the assumption that p 
is negligible, (A possible fix would be to rotate 
the single rate gyro so that components of both roll 
rate and yaw rate are measured.) 

It is certainly possible that the performance achieved from 
Methods 3 and 4 could be improved by gain changes and software 
modifications. However, such efforts should await more exten- 
sive flight data to work with. 


TABLE 7.- COMPARISON OF ESTIMATOR METHODS USING THE 

402B DATA 


ANGLE, 0 

DEG c 

m a 

9 0 1.58 

\Jf -0.034 1.38 


METHOD 

3 


m a 

-0.20 4.56 

0.12 4.30 


4 


m a 

0.23 10.55 

-1.81 25.03 
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Conclusion 


Based on the results of processing the CV-990 and Cessna 
402B data, it is concluded that Method 2 is the preferred 
estimator structure. Modification can be made to this method 
so that the complementary filter software which determines 
pi-tch angle can be elimi-na-t-ed. That is, pitch angie can be 
computed directly by using Eqs. (3) and (10). This also 
eliminates need of the pitch accelerometer. 

Also based on these results, it can be concluded that one 
can potentially estimate the three orientation angles of the 
aircraft to a high degree of accuracy. The estimated roll 
and heading angle trajectories obtained using the Cessna 402B 
data resembled very closely the angles directly measured using 
the INS This is very encouraging. The possible problems 
which may arise due to the ambiguity m solution for roll 
angle in Method 2 will have to be discovered or dismissed by 
much more extensive flight testing. 
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IV 


SOFTWARE MECHANIZATION REQUIREMENTS 


An essential requirement for mechanizing the state esti- 
mator concepts is that their software can be successfully 
coded in a typical microcomputer with the associated con- 
straints on memory, computation accuracy, and cycle time. To 
make an assessment of to what degree this requirement can be 
met, the following study was made using a DEC PDP 11/70 com- 
puter. 

First, the FORTRAN software required to mechanize the 
Method No. 2 state estimator was extracted from the ESTEST 
program described in Appendix A. (The ESTEST program was 
developed to test all estimator configurations using flight 
data.) The Method 2 estimator configuration is the longest 
but most accurate of the configurations studied. The Method 
2 FORTRAN code is presented and explained in Appendix B. 

Next, the portion of the software which represents com- 
putations made every cycle of the mechanized estimator were 
recoded on the PDP 11/70. Phases of these computations 
include : 

(1) modification of sensor readings to remove known 
errors; 

(2) computation of independent measures of the attitude 
angles (<p, 9, \j/) from magnetometer and other 
readings; and 

(3) primary state estimator (filter) computations. 

Also, there would be a small amount of additional software 
for input and output conversion. The program was coded using 
the C language of the UNIX system (developed by D.M. Ritchie, 
Bell Laboratories) which provides efficient, compact PDP 11 
code. A listing of the C source deck is also presented m 
Appendix B. 

The C source deck was compiled and assembled into PDP 11 
machine language using the floating point instruction set 
The associated memory requirements for this program were 2024 ^q 

sixteen bit words. This requirement could be reduced about 25% 
using fixed point arithmetic. However, fixed point arithmetic 
would require addition of some scaling operations. 

Some additional software would be required to make the 
initial computations at the beginning of use of the estimator. 
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These computations are presented m Appendix B. Also, execu- 
tive logic would be required to sample and scale the A/D 
buffer inputs, to interface with pilot inputs, to prepare the 
estimate outputs for display or digital control, and to con- 
trol program cycling. A conservative estimate is that this 
would add 50% to the memory requirements. Thus., it is seen- 
th”at the read only memory (ROM) requirements for mechanization 
are much less than 4096 (4K) sixteen-bit words. With efficient 
coding, this could be reduced to 2K words. 

The variable storage requirements (RAM) for mechanization 
is sixteen-bit words. Thus, a 256 x 16-bit RAM unit is 

adequate . 

The tests made using the Cessna 402 discussed in the pre- 
vious chapter used data with ten-bit accuracy. The estimate 
results 'were quite adequate in comparison to INS measurements. 
Thus, it can be concluded that a microcomputer with twelve or 
sixteen-bit words is adequate for mechanizing the state esti- 
mator. Further tests would be required to evaluate the 
adequacy of an eight-bit processor. 

To obtain an estimate of computation time, a single pass 
through the PDP 11 computations was timed. The result was 
less than 16 7 msec. (The minimum measurable time increment 
is 1/60 sec. ) Norden Corporation personnel estimated that the 
time increase would be a factor of five (to less than 83.5 
msec) for running on the LSI11M microcomputer which uses the 
PDP" 11 code. The LSI11M is a ruggedized microcomputer suitable 
for airborne application. 

For processing the flight test data, sample rates of once 
per second (CV-990 data) and fourteen times per second (C-402 
data) were used. The Method 2 estimator worked well in both 
cases. Thus, a sample rate of five times/sec appears to be 
adequate. 

If the cycle time were set at 200 msec so that the 
sensors were sampled five times /sec, it is seen that the 
basic computations of the state estimator would require only 
41% of real time for the LSI11M Again, to be conservative, 
this value could be increased 50% to account for additional 
executive computations. There appears to be plenty of margin 
for running time. 

The above study is a first approximation to the micro- 
computer mechanization requirements. From these, it can be 
concluded that the state estimator concept can easily be 
mechanized m existing microcomputers. To obtain more precise 
timing and memory requirements requires actual mechanization 
on a microcomputer with additional software added for driving 
A/D converters, displays, and the program control logic. 
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V 


SUMMARY , CONCLUSIONS, AND RECOMMENDATIONS 


Summary 

This study has accomplished the following: 

(1) Pour nonlinear state estimators (Methods 1, 2, 3, 
and 4) were devised which provide techniques for 
obtaining the angular orientation of the aircraft. 
These techniques are alternatives to direct measure- 
ment by use of vertical and directional gyros. 

These estimators have the potential of being of low 
cost and of high reliability by implementation using 
solid state instruments (pressure sensors, acceler- 
ometers, magnetometers) and the microcomputer. 

(2) An extensive FORTRAN computer program was developed 
to demonstrate and evaluate the estimators by pro- 
cessing recorded flight test data. This program 
simulates the estimator operation and it compares 
the state estimates with actual state measurements. 
Full details and capabilities of this program are 
presented in Appendix A. 

(3) The above program was used to evaluate the four 
state estimator configurations with limited data 
recorded on the NASA Ames CV-990 and Cessna 402B 
aircraft. Three of the configurations worked reason- 
ably well with the CV-990 data and Method 2 worked 
well with the 402B data. From these evaluations, 

the preferred state estimator configuration was 
chosen. 

(4) A preliminary assessment was made of the require- 
ments for implementing the selected state estimator 
on a typical microcomputer. 


Conclusions 

Based on limited flight data analysis, it is concluded 
that the estimator concept of determining attitude angles 
without direct measurement has definite potential to provide 
low-cost flight control The measurements required to esti- 
mate roll, pitch, and yaw angles include the three components 

of magnetic field (B , B , B ) three components of linear 

x y z 

acceleration (f , f , f„), two components of angular accelera- 
« • ^ y z 

tion (p, r), true airspeed (V_), baro-altitude (h), and 

ci 
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possibly flap angle (SF). Angular acceleration could be re- 
placed by angular rate (p, r) measurements. Altitude and true 
airspeed measurements can be obtained by processing static 
and dynamic pressure data. 

It is furthermore concluded that the above set of measure- 
ments, which are smoothed by nonlinear filtering in the esti- 
mator, can provide attitude angle estimates to a high degree 
of accuracy. For example, during an 80 sec run using Cessna 
402B data, the roll angle excursions of the aircraft exceeded 
45° and the yaw angle excursions exceeded 120° . During this 
time, the estimated roll angle matched the INS measured roll 
angle to a mean of 0° and a standard deviation of 1.6°. The 
estimated yaw angle matched the INS measured yaw angle to a 
mean of -0.3° and a standard deviation of 1.4°. This accuracy 
is more than adequate for flight control purposes. 

The selected state estimator configuration (Method 2) has 
a potential ambiguity problem m determining roll angle when 
the aircraft is flying at a magnetic heading of nearly North 
or South. This problem was not encountered with the limited 
data processed in this study. The solutions to this problem 
(encompassed in Methods 3 and 4) degrade the estimator perfor- 
mance in the presence of aircraft transient attitude motion. 

The degree of degradation is dependent upon the instrumentation 
errors and the amount of disturbances causing transient motion 
of the aircraft. 

The computation mechanization requirements for implement- 
ing the Method 2 state estimator are easily met with today's 
microcomputers. Preliminary conservative estimates are that 
to code this estimator on a ruggedized microcomputer requires 
less than 4K x 16-bit BOM memory and less than 256 x 16-bit RAM 
memory. Twelve-bit memory is also sufficient. A preliminary 
timing assessment indicated that less than 0.1 sec is required 
to cycle the estimator equations on the ruggedized microcomputer. 
This allows a sample rate of flve/sec with plenty of time to 
spare for either driving displays or automatic control actua- 
tion. 


Recommendations 

The results of this study were based on limited flight 
data. The data collected did not represent all the flight 
regimes m which the estimators would potentially have to 
operate. In particular, flight data in which several turns, 
intentional lateral and longitudinal perturbations, known wind 
shears, stalls, and engine-out conditions were not tested. It 
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is recommended that such data be collected on the Cessna 402B 
aircraft and that a more thorough investigation be made. 

It is also recommended that the following steps be taken 

(1) Estimator Methods 2, 3, and 4 are based on computing 
pitch angle from measurements of vertical accelera- 
tion, dynamic pressure, and flap angle and estimates 
of aircraft mass and lift coefficient. Study should 
be directed to find a more direct procedure to 
compute pitch so that at least flap angle measure- 
ments could be eliminated. 

(2) Estimator Method 2 gave the best results in the 
studies made. However, it has a potential ambiguity 
problem in computing roll angle when the aircraft is 
flying with a magnetic heading of near 0° or 180° . 

This potential problem must be thoroughly investigated 
with flight test data, and further corrective logic 
may be required. 

(3) Intentional instrumentation errors should be artifi- 
cially introduced into the program used to process the 
flight data and to evaluate the estimators. The 
sensitivity of the state estimator outputs to instru- 
ment error magnitude can then be determined. This 
procedure is suitable for specifying required instru- 
ment accuracy. These results would complement 
laboratory studies of existing low-cost solid state 
sensors, 

(4) A typical microcomputer should be selected along with 
appropriate sensor interface equipment, recording 
equipment, and operating peripherals. Additional 
software should then be developed to sample the sensor 
input, provide program control, and drive outputs 

for data recording (or display). The entire software 
code of the selected estimator configuration should 
then be loaded into the microcomputer, and subsequent 
tests should be made to obtain more definitive require- 
ments for computer mechanization. 

Further concept study of low-cost state estimation for 
flight control should produce 

(1) A precise definition of the desired state estimator 
configuration and any limitations it has. 

(2) The computer, sensor, and display mechanization 
requirements to realize this concept. These include 
accuracy requirements of both the computer and 
sensors 
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APPENDIX A 


PROGRAM USER'S GUIDE 


As part of this study effort, a FORTRAN digital computer 
program (ESTEST) was developed for the Ames IBM 360 to process 
the flight data. The purpose of this program is to simulate 
operation of the state estimators used for flight control pur- 
poses. The flight data serves as input to drive this simula- 
tion. The state estimates are compared to actual state vari- 
ables obtained by direct measurement to assess the estimator 
performance. 

The ESTEST program allows the user to make the following 
studies: 

(1) Digital implementation of the state estimator can 
be checked. 

(2) Different estimator formulations can be tested and 
compared. 

(3) Performance of the estimator can be measured for 
different flight conditions and sensor accuracies. 

(4) Gains and other program variables can be adjusted. 

(5) Airborne computer requirements for mechanization 
can be partially assessed. 

This appendix serves as a user’s guide for ESTEST. It is 
organized as follows. 

Cl) The input variables are defined, and a sample input 
deck is listed. 

(2) The program output is explained, and sample outputs 
are presented. 

(3) The general capabilities and organization of the 
program are explained. 


Input Variables 

Five input formats are used to read in the initial data 
and program control variables. These formats are. 
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(1) F0RMAT(2X, 513) , 

(2) FORMAT ( 2 0A4) , 

C3) F0RMAT(2X,6I3), 

(4) FORMAT (616), and 

(5) FORMAT ( 2X , 6E12 . 5 ) . 

T-went-y-ea.-ght input- data cards- are read using these formats 
to initialize operation of the program. They are presented in 
Table A.l as they are arranged in the data card set; the above 
formats are referenced. The definitions of these variables are 
presented in Table A. 2. Figure A.l shows a listing of a typi- 
cal input deck, with the appropriate Ames IBM 360 control cards. 

After the initialization data and control variables have 
been read in, ESTEST immediately prints this data. This is 
discussed m the next section. Then, certain initialization 
computations are made such as conversion from degrees to 
radians. Then, the time data set is read sequentially using 
the following FORTRAN statements* 


READ(8) NTIME 
50 CONTINUE 

READ(8) K , ( CVDAT ( J ) , J=1 , 17 ) 
IF (K.LT.NST) G 0 T 0 50. 


This is explained as follows: 

(1) The number of time points of data m the data set 
(NTIME) is first read. 

(2) Each sequential data time point record is then read 
until the index K is equal to the input quantity 
NST which is the start point desired. 

The quantities in each data time record represented by the 
array CVDAT are defined m Table A. 3. The program is set by 
the logic variable NF1 so that either data collected on the 
CV-990 aircraft or the Cessna 402B aircraft can be used. The 
differences beween the CV-990 and C-402 data arrays are indi- 
cated in Table A. 3. 


Program Output 

The first thing the program does after reading in the 
run initialization data is to print it. A sample of this 
printout is shown in Fig. A. 2. Each of the variables is 
defined by the preceding acronym which is defined in Table A. 2. 
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TABLE A.l - SEQUENCE OF INITIAL DATA AND PROGRAM CONTROL 
VARIABLES READ TO INITIALIZE ESTEST OPERATION 


CARD 

FORMAT 

FORTRAN ACRONYMS 

1 

1 

( I DATE ( I ) , 1=1,3), NR, MAC 

2 

2 

(ALPHA(I), 1=1,20) 

3 

3 

NF1, NF2, NF3, NF4, NF5, NF6 

4 

3 

NF7, NF8, NF9, NF10, NF11 

5 

4 

NS, NST, NRU, IX 

6 

5 

BMAG, DL1, DL2 

7 

5 

FIB, THB, SIB, SIBY, THBZ, FIBZ 

8 

5 

BBX, BBY, BBZ, EPBX, EPBY, EPBZ 

9 

5 

SGBX, SGBY , SGBZ 

10 

5 

DT, TST0P, TI, DTP, DTPL, DTST 

11 

5 

FIPD, THPD, SIPD, EPPD, EPQD, EPRD 

12 

5 

BPD, BQD, BRD, SGPD, SGQD, SGRD 

13 

5 

BV, EPV, SGV 

14 

5 

FIA, THA, SIA, XA, YA, ZA 

15 

5 

BAX, BAY, BAZ, EPAX, EPAY, EPAZ 

16 

5 

SGAX, SGAY, SGAZ 

17 

5 

BH, EPH, SGH 

18 

5 

FIAM, THAM, SIAM, XAM, YAM, ZAM 

19 

5 

THV, RKU, RKSB, RKB, DFBI , DFSF 

20 

5 

FIAL, THAL, SIAL, RKA1, RKA2 

21 

5 

RL, RKGX, RKGY, RKGZ, RKB1 

22 

5 

FGL, G 

23 

5 

SICBX, THCBX, SICBY, FICBY, THCBZ, FICBZ 

24 

5 

RM, CZAL, SW, ALZR0, ALZR1, H0 

25 

5 

RK1, RK2, RK3, RK4, RK5, RK6 

26 

5 

RK7, RK8, RKBH, RKBP, RKBQ, RKBR 

27 

5 

BAXC, BAYC, 8AZC, EAXC, EAYC, EAZC 

28 

5 

BCBX, BCBY, BCBZ, ECBX, ECBY, ECBZ 


49 






TABLE A. 2.- DEFINITION INPUT DATA CARD VARIABLES LISTED 

IN TABLE A. 1 


CARD ACRONYM SYMBOL 

1 I DATE (I') 

NR 

NAC 

2 ALPHA(I) 

3 NF1 
NF2 

NF3 


NF4 

NF5 

NF6 


4 NF7 


NF8 


DEFINITION 


Date in month/ day /year that data were 
taken 

Estimator (or run) trial number 

Type of aircraft (i.e., 990, 402). 

120 characters used to identify a par- 
ticular run 

Data source: 1 - CV-99Q 

2 - C-402B 

Magnetic data used: 

1 - Computed from INS angles 

2 - Actual magnetometer 

Simulated sensor errors introduced: 

0 - None 

1 - Deterministic 

2 - Deterministic + random 

Sensor corrections used: 

0 - None 

1 - Corrections 

Airspeed measurement source: 

1 - Pitot tube 

2 - J-Tek sensor 

Method of computing cp, 9 S and >|r: 

1 - Method 1 

2 - Method 2 

3 - Method 3 

4 - Method 4 

Computer printout: 

0 - None 

1 - Major 

2 - Major + secondary 

Computer plot: 

0 - No plot 

1 - Plot 
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TABLE A, 2 (Continued) 


ACRONYM 

SYMBOL 

DEFINITION 

NF9 


Ames Zeta plot: 0 - No plot data 

1 - Plot data saved 

NF10 


Statistical measures: 0 - Not computed 

1 - Computed 

NF11 


9 , \J/ computations only: 0 - Option off 

1 - Option on 

NS 


Number of samples to skip between use 

NST 


Data set index number used to indicate 
start of data of interest 

NRU 


Number of cases to be run from data 
set. 

IX 


Initial add number for random number 
generator. 

BMAG 

B mag 

Assumed or actual magnitude of the 
local magnetic field (milligauss) 

DL1 

6 L1 

Deviation of magnetic north from true 
north (deg) 

DL2 

S L2 

Magnetic field dip angle (deg) 

FIB 

THB 

SIB 

V e B * B 

Simulated magnetometer misalignment 
angles (deg) 

SIBY 

THBZ 

FIBZ 

%y’ 0 Bz’ 
^Bz 

Simulated magnetometer skew angles; 

B with respect to B and B with 
y x 4 

respect to B-B plane (deg) 
x y 

BBX 

BBY 

BBZ 

b Bx* b By’ 
b Bz 

Simulated biases of magnetometer 
readings (milligauss) 

EPBX 

EPBY 

EPBZ 

e Bx’ e By’ 
e Bz 

Simulated magnetometer scale factor 
errors; (1+s) multiplies the simulated 
signal 
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TABLE A. 2 (Continued) 


CARD 

ACRONYM 

SYMBOL 

DEFINITION 

9 

SGBX 

- ~Bx L~By * 

ft 

Standard deviations used by random num- 


SGBY 

ber subroutine for simulated magnet- 


SGBZ 

Bz 

ometer noise (milligauss) 

10 

DT 

At 

time between samples (sec) 


TSTOP 

^stop 

Length of time duration of data 
sequence used in the run (sec) 


TI 

t I 

Time from beginning of data record to 



point where run begins; corresponds to 
NST (sec) 



DTP 

At p 

Print interval (sec) 


DTPL 

At p* 

Plot interval (sec) 


DTST 

At st 

Interval for computing estimate devia- 
tion means and variances (sec) 

11 

FIPD 

V e p J 

v p 

Simulated angular accelerometer mis- 


THPD 

alignment angles (deg) 


SIPD 



EPPD 

W 

Simulated angular accelerometer scale 


EPQD 

factor errors 


EPRD 

& * 

r 


12 

BPD 

b o’ b a> 

Simulated angular accelerometer biases 


BQD 

BRD 

P 9 

b* 

r, 

(rad/sec ) 


SGPD 

a n s °o» 

Standard deviations used by random num- 


SGQD 

P W 

ber subroutine for simulated angular 


SGRD 

°r 

accelerometer noise (rad/sec2) 

13 

BV 

b v 

Simulated airspeed measurement bias 



V 

(ft/sec) 


EPV 

e v 

Simulated airspeed measurement scale 




factor error 


SGV 


Standard deviation used by random num- 



V 

ber subroutine for simulated airspeed 
measurement noise (ft/ sec) 
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TABLE A. 2 (Continued) 


CARD 

ACRONYM 

SYMBOL 

14 

FIA 

Cp.» 0 a> 


THA 

a a 

,t- 


SIA 

*a 


XA 

x_. 


YA 

z a 


ZA 

z a 

15 

BAX 

b . b . 


BAY 

ax’ ay’ 


BAZ 

b az 


EPAX 

s * s ■ 


EPAY 

ax ay’ 


EPAZ 

B az 

16 

SGAX 

SGAY 

a ax’ °ay’ 


SGAZ 

a az 

17 

BH 

b h 


EPH 

s h 


SGH 

a h 

18 

J 

FI AM 
THAM 

^am’ 9 am’ 

j. 


SIAM 

^am 


XAM 

x . y_ j 


YAM 

am’ J am’ 


ZAM 

z am 

19 

THV 

9 v 


RKU 

K u 


RKSB 

jr 

CO 


DEFINITION 


Simulated linear accelerometer mis- 
alignment angles (deg) 


Simulated position of linear acceler- 
ometers with respect to the aircraft 
c.g. 

Simulated linear accelerometer biases 
(ft/sec 2 ) 

Simulated linear accelerometer scale 
factor errors 


Standard deviations used by random num- 
ber subroutine for simulated linear 
accelerometer measurement noise 

(ft/sec^) 

Simulated altimeter bias (ft) 

Simulated altimeter scale factor error 

Standard deviation used by random num- 
ber subroutine for simulated altimeter 
measurement noise (ft) 

Linear accelerometer misalignment cor- 
rection angles (deg) 


Linear accelerometer position correc- 
tion terms with respect to the c.g. 
(ft) 

Pitot tube misalignment correction 
angle (deg) 

Gain for airspeed filtering 

Gain for filtering gxB terms in 
Method 1 
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TABLE A, 2 CContmued) 


CARD 

ACRONYM 

SYMBOL 

DEFINITION 

19 

- TKB 

h - 

- Gain for filtering B in Method- 2 - 


DFBI 

b SF 

Flap angle bias (deg) 


DFSF 

C SF 

Flap angle scale factor error. 

20 

FIAL 

V V 
% 

Angular accelerometer misalignment cor- 


THAL 

rection angles (deg) 


SIAL 


RKA1 

RKA2 

K al- K a2 

Extra gains 

21 

RL 


Turn rate limit for update of g in 



L 

Method 1 (rad/sec) 


RKGX 


Gams for updating g from linear 


RKGY 

9 x gy 
K gz 

accelerometer readings in Method 1 


RKGZ 


RKB1 

*bi 

Gain used by digital lag network to 
smooth rate gyro data 

22 

FGL 

f 

g2 

Threshold limit on accelerometer 
readings used to compute g in Method 

1 (ft/sec 2 ) 


G 

g 

O 

Normal gravity value (ft/sec ) 

23 

SICBX 

THCBX 

SICBY 

G Bx’ 
V 9 By’ 

Magnetometer axis misalignment correc- 
tion angles (deg) 


FICBY 

THCBZ 

FICBZ 

0 8z s ^z 


24 

RM 

m 

Aircraft mass (slugs) 


CZAL 

C Z a 

Aircraft lift coefficient as a function 
of angle-of-attack 


SW 

S 

p 

Aircraft wing reference area (ft ) 


54 




TABLE A. 2 (Continued) 


CARD 

ACRONYM 

SYMBOL 

DEFINITION 

24 

ALZRO 

“o’ “l 

Terms used to compute zero lift angle- 


ALZR1 


of-attack as a function of flap angle 
(rad, rad/rad) 


H0 

h o 

Barometric altimeter correction (ft) 

25 

RK1-RK6 


Primary filter gains for estimating 




altitude, roll, and pitch (or angle-of- 
attack) 

26 

RK7, RK 8 

K 7 , Kg 

Primary filter gains for estimating 
yaw 


RKBH 

K bh’ K bp’ 
K bq* K br 

Primary filter gains for estimating 


RKBP 

biases in altitude, roll acceleration, 


RKBQ 

pitch acceleration, and yaw accelera- 


RKBR 

tion 

27 

BAXC 

b » b • 

Linear accelerometer bias correction 


BAYC 

BAZC 

axe aye 
b azc 

p 

terms (ft/sec ) 


EAXC 

^ 3 V/ 1 ^ ^ 9 WO ^ 

Linear accelerometer scale factor 


EAYC 

axe aye 

correction terms 


EAZC 

s azc 


28 

BCBX 

b cbx* b cby’ 

h 

Bias corrections to magnetometer sig- 


BCBY 

nals (milliguass) 


BCBZ 

b cbz 



ECBX 

ECBY 

E cbx’ s cby’ 

Magnetometer scale factor error correc- 
tions 


ECBZ 

e cbz 
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FIGURE A. 1 . - LISTING OF TYPICAL ESTEST INITIAL DATA INPUT DECK 



TABLE A. 3.- QUANTITIES IN THE DATA INPUT ARRAY CVDAT 


NO, 

SYMBOL 

DEFINITION (CV-990) 

ALTERNATE DEFINITION (C-402) 

1 

t 

Time (sec) 


2 

<P 

INS roll angle (deg) 


3 

e 

INS pitch angle (deg) 


4 

+ 

Dir. gyro yaw angle (deg) 

- INS yaw angle (deg) 

5 

p 

Roll rate (deg/sec) 


6 

q 

Pitch rate (deg/sec) 


7 

r 

Yaw rate (deg/sec) 


8 

h 

Altitude (ft) 


9 

V a 

J-Tek true airspeed (kts) 


10 

r 

Flight path angle (deg) 

B - Longitudinal magneto- 
x meter reading (milli- 
gauss) 

11 

f x 

Longitudinal acceleration 
(ft/ sec 2 ) 


12 

f y 

Side acceleration (ft/sec 2 ) 


13 

f z 

Vertical acceleration 




(ft/ sec 2 ) 


14 

W ind 

Wind magnitude (kts) 

B - Lateral magnetometer 
^ reading (milligauss) 

15 

% 

Wind angle (deg) 

B - Vertical magnetometer 
reading (milligauss) 

16 

SF 

Flap angle (deg) 

SF^ - Left flap angle (deg) 

17 

6T 

Throttle setting {%) 

SF d - Right flap angle 
(deg) 
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FIGURE A. 2.- PROGRAM PRINTOUT OF INPUT DATA 






The program has the option (using the NF7 flag) of print- 
ing out no data, major data, or major plus secondary data every 
DTP seconds. The data heading and a sample of the printout 
data are shown in Pig. A. 3. The major data are the first two 
lines at each time point. The secondary data are the next 
three lines. The acronyms shown in the data header define 
what the quantities in each data set are. These header acro- 
nyms are defined in Table A. 4. The estimated quantities (cp, 

Pi q, r) in the second line of the printout appear directly 
below the measured quantities of the first line. 

Another option which the program has (using the NP10 flag) 
is computation of ^statistical characteristics of the estimated 
variables (cp, 0, p, q, r, fi, $ ) as compared to the directly 

SI 

measured variables. An example output of these measures is 
shown in Pig. A. 4. The mean difference, the variance about 
this mean, and the resultant standard deviation are computed 
for the entire length of the run. The first three variables 
are the roll, pitch, and yaw estimates compared to the INS 
measurements. The second three variables (nos. 4, 5, and 6) 
are the roll, pitch, and yaw rate estimates compared to the 
rate gyro measurements. The seventh line compares the smoothed 
altitude to the barometric altimeter measurements. The eighth 
line compares the smoothed airspeed to the J-Tek true airspeed 
measurement . 

A third option of the program (using Option Flag NP8) is 
to produce computer generated plots of the estimated and 
measured attitude angles and rates. An example plot of the 
roll angle is shown in Fig. A. 5. In this plot, the asterisk 
(*) represents the INS measurement of roll angle cp. The 
0 is the estimated roll angle cp. The plus sign ( + ) is the 
difference which is used to compute the statistical measures. 
These plots are automatically scaled so that the width of the 
entire page is used. 

A fourth option of the program (using Option Flag NF9) is 
to produce plots from the Ames Zeta plotter. This option 
writes a data set, and the plot is produced by use of a dif- 
ferent program. An example of this type of plot is shown in 
Fig. A. 6. 


Program Explanation 

Overview . - An overview of the ESTEST program is repre- 
sented by the block diagram m Fig. A. 7. After the initializa- 
tion calculations have been made and the data set has been 
advanced to the desired starting point, the program enters an 
iterative loop It remains m this loop until the last desired 
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FIGURE A. 3.- OUTPUT DATA HEADER 
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TABLE A. 4.- DEFINITION OF ACRONYMS IN DATA HEADER OF 

FIGURE A. 3 


LINE 

ACRONYM 

EXPLANATION 

X 

T 

Time from beginning of run (sec) 


FI 

Roll angle from INS (deg) 


THET 

Pitch angle from INS (deg) 


SI 

Heading {or yaw) angle from INS {or directional gyro) 
(deg) 


P 

Rate gyro measured roll rate (deg/sec) 


Q 

Rate gyro measured pitch rate (deg/sec) 


R 

Rate gyro measured yaw rate (deg/sec) 


FX 

Longitudinal acceleration (ft/sec 2 or g's) 


FY 

Lateral acceleration (ft/sec or g's) 


FZ 

Vertical acceleration (ft/sec^ or g's) 


VI 

J-Tek measured true airspeed (ft/sec) 

2 

FIH 

Estimated roll angle (deg) 


THTH 

Estimated pitch angle (deg) 


SIH 

Estimated yaw angle (deg) 


PH 

Estimated roll rate (deg/ sec) 


QH 

Estimated pitch rate (deg/sec) 


RH 

Estimated yaw rate (deg/sec) 


H 

Barometric altitude (ft) 


HH 

Smoothed altitude (ft) 


ALH 

Estimated angle-of-attack (deg) 


VAH 

Smoothed true airspeed (ft/sec) 
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TABLE A. 4 (Continued) 


LINE ACRONYM 

3 FIM - 
THIM 

SIM 

PDM 

QDM 

RDM 

BX 

BY 


EXPLANATION 


RoTl-angle determined from magnetometer -data (deg)" 

Pitch angle determined from magnetometer or acceler- 
ometer and dynamic pressure data (deg) 

Yaw angle determined from magnetometer data (deg) 

Derived roll acceleration (deg/sec^) 

p 

Derived pitch acceleration (deg/sec ) 

2 

Derived yaw acceleration (deg/sec ) 

Measured or derived longitudinal component of magnetic 
field 

Measured or derived lateral component of magnetic 
field 


BZ 

HHD 

4 GXH 
GYH 
GZH 
GAM 
QH 
AMN 

BXH 

BYH 

BZH 


Measured or derived vertical component of magnetic 
field 

Estimated altitude rate (ft/sec) 

Estimated longitudinal component of gravity (ft/sec^) 
Estimated lateral component of gravity (ft/sec^) 

p 

Estimated vertical component of gravity (ft/sec ) 

Flight path angle (deg) 

Derived dynamic pressure (lb/ft^) 

Measured angle-of-attack as function of dynamic pres- 
sure, vertical acceleration, and flap angle (deg) 

Smoothed longitudinal component of magnetic field 

Smoothed lateral component of magnetic field 

Smoothed vertical component of magnetic field 
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TABLE A. 4 (Continued) 


LINE ACRONYM 

4 FL 

5 BHH 


BHP 

BHQ 

BHR 


EXPLANATION 

Flap angle (deg) 

Estimated acceleration bias in altitude filter 
(ft/sec 2 ) 

Estimated acceleration bias in roll filter (deg/sec ) 
Estimated acceleration bias in pitch filter { deg/sec 2 ) 
Estimated acceleration bias in yaw filter (deg/sec ) 
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STATISTICAL PERFORMANCE MEASURES 
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FIGURE A. 4.- EXAMPLE PROGRAM STATISTICAL PERFORMANCE MEASURES 
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FIGURE A. 5.- (CONTINUED) 
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FIGURE A. 7.- PROGRAM FLOW CHART 
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data point has been processed, Reading the data is similar 
to taking sampled measurements from the on board sensors. 

Following the reading of the data, the option is avail- 
able to add artificial errors to these data. This allows the 
user to investigate, for any data sequence, the effect of the 
errors on the estimation accuracy. In this way, the sensor 
accuracy can be specified. 

In addition to adding artificial errors, artificial sen- 
sor signals may also be generated. For example, for the CV-990 
data, artificial magnetometer readings were generated as a 
function of the INS and directional gyro mreasurements of 
9 , +). 

After the sampled measurement signals are prepared, the 
program enters a block which represents a replica of the 
digital state estimator which would be implemented on board 
the aircraft. This has five steps, as indicated m Fig. A. 7. 
The signal adjustment, attitude angle, and primary filter 
computations are discussed in detail m Chapter II. Outputting 
the state estimates is analogous to using the estimates for 
cockpit display or to drive a flight director or autopilot. 

When the final data point has been processed, the statis- 
tical measures presented in Fig. A. 4 are computed. Also, the 
resulting plots, such as shown in Figs. A. 5 and A. 6, are pre- 
pared. 

Artificial signal generation .- There are four sets of 
measurements which may be required to be generated artificially 
from other sensor readings. These include the magnetic field, 
the angular acceleration, the true airspeed, and the dynamic 
pressure. 

Generation of the three components of the magnetic field 
requires knowledge of the vector magnitude ® ma g an d dip 

angle Sg. Then the magnetic north and downward components of 

the field are, respectively, 


B 

xo 


B 


mag 


cos 6 2 


B 


zo 


B 


mag 


sin 6 2 


(A.l) 


From the INS (and possibly the directional gyro), the 
roll, pitch, and yaw angles (<p, 0, \jf) of the aircraft are read. 
From these, the three body-fixed components of the magnetic 
field are computed to be 
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B = B cos 0 cos \j/ - B sm 9, 
x xo T zo ’ 

By - B xo (sirt 9 sin 0 cos - cos 9 sin ij/) 

+ B sm 9 cos 8 , 

ZO _ ^ ’ _ - - 

B = B (cos cp sm 0 cos t + sm 9 sm \lf) 

Z XO Y T x T/ 

+ B cos 9 cos 0. (A. 2) 

zo 

The rate gyro measurements (p, q, r) are used to generate 
artificial angular accelerometer data. For the Cessna 402B 
data where unsmoothed gyro samples were taken approximately 
every 0.07 sec, the samples were first smoothed using a simple 
lag filter, e. g. , 

P f “ k Bl (p ~ p f > (A. 3) 

Here, p^ is the smoothed value of the measured roll rate p, 
and is the inverse time constant of the filter. This is 

implemented digitally as 

Pf+l “ k Bl At(p n+l- p f )+p f • CA ' 4) 

Here, p is the measured rate at the n+1 time point. 
n +1 

Also, p f and indicate the filtered rates at the time 

points n and n+1. Then, the artificial angular acceleration is 
computed as a simple difference; e.g., 


P * (P f+i - P f )/At . (A. 5) 

In Eqs. (A. 4) and (A. 5), At is the sample time. For the 
CV-990 data, where 20 samples were added and averaged to pro- 
duce data points approximately 1 sec apart, Eq. (A. 5) was used 
directly without first filtering. 

If pitot tube measurements are used, conversion is neces- 
sary from indicated (V ) to true airspeed (V_). Incompressible 

m el 

flow is assumed, so that 

V = V Spjp « V S. (A. 6 ) 

a m o' r m 
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The quantity S (= l/v'o = /p Q /p ) is computed using linear 

interpolation from a table with quantities as a function of 
smoothed altitude fi. The interpolation equation is 


(S H“ S L> ^ 
s l + o^y (fi ' h L> 


S L + S oh' h - h L’ 


(A. 7) 


The quantities S^, S Q j 1 , and h^ are presented in Table 5. 

If dynamic pressure (Q) measurements are required, they 
are generated from the smoothed true airspeed using the equa- 
tion 


§ - 0.001189 c V? (A. 8) 

Here, the quantity cr again comes from linear interpolation 
as a function of altitude: 

c = + a^Ch-h^) * ' (A. 9) 

The quantities and a ^ also appear in Table A. 5. This 

table and Eqs. (A.6)-(A.9) appear in subroutines CONV and CONV1. 


TABLE A. 5.- TABLE LOOKUP QUANTITIES USED TO COMPUTE TRUE 
AIRSPEED AND DYNAMIC PRESSURE 


ALTITUDE 
h L , FT 

DENSITY 
RATIO S L 

S oh’ , 
1/FT x 10° 

DENSITY 
RATIO a L 

°oh> 4 
1/FT x HT 


1.0 


1.0 



1.02991 

1.4955 

0.94277 

-0.28615 

5000 

1.07728 

1.5790 

0.86167 

-0.27033 

10000 

1.16367 

1.7278 

0.73848 

-0.24638 
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Artificial error generation - The option flag NF3 is used 
to indicate whether artificial errors are added to the signal 
measurements. 

For the magnetometer readings, the signals (B B ,B ) are 

X y z 

-- subject to sensor mfsaTrgnment angles (9g, t|/g) , "triad skew 

angles (\Jf B , 0 Bz , 9^), biases (b Bx , b By , b Bz ), scale factor 
errors (e Bx , s By , e Bz ), and noise terms ("n Bx , ri Byl rj Bz ) . The 
equations which introduce these errors are: 


B" = B + ij/ D B - e„ B , 
— x Y B y B z ’ 


xm 


B" 


ym 


B" 


zm 


B" = B 
xm 


B" 


ym 


B" 


zm 


B. 


xm 


B 


ym 


*+B B x + B y + ^ ’ 

S B B x ‘ <p B B y + B z ■ 

* 

xm ’ 

-ifo B"* + B" , 

T oy xm ym ’ 

®Bz B xm ~ ^z^ym + B zm ’ 

(1 + + b Bx + t, Bx , 

(1 + s By> B y« + b By + 1 By - 


B zm “ (1 + e B z ) E ; ra + b B z + 1 Bs 


(A. 10) 


The noise terms (rj B , rj B , rj« ) are generated each sample point 

by using a random number generator with assumed Gaussian 
statistics and standard deviations (a Bx , a By j a Bz^' Obher 

noise terms subsequently mentioned are generated in a similar 
manner. 

The angular accelerometer signals (p , q , r ) are subject 

n n n 

to misalignment angles ( cp- , 9*, scale factor errors (s*, 

P P P P 

e q> s^), biases (b^, b^, bp, and noise terms (e^, e^, sp. 

These are introduced as 
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(A. 11) 


p ra = (1 + e p )(p n + Vn-Vn 5 + b i + ”p ’ 

«m = (1 + 6 q ) <-’t , p p n + «n + < f , p i 'n ) + b q + ’Iq ' 

r_ = (1 + e • )(0*p - <p*6 + r„) + b* + rj» . 
m ? p n T p n n r *r 

The linear accelerometer signals (f ,f ,f ) are subject 

a y z 

to position errors with respect to the aircraft center of 
gravity (x.jy.jZ.), misalignment angles (<p Q ,B ) , scale fac- 

ci cL ci cL a, ci 

tor errors ( s ax > e ay > s az ) i biases (b ax ,b ay J b az ) , and noise terms 
^ax ,Ti ay ,ri az^* The error equations are 

• • 2 2 

f" =3 q z — r y - (a + r )x + p (q y + r z ) + f , 

x 4 n a n y a V4 n n' a *Wa n a' x * 

• • 2 2 

= r n x a - p n z a “ <p n + V y a + 9 n Cp n x a + Va* + f y • 

f z = p n y a “ V» ' Cp n + «n )z a + V p n x a + Va 5 + f z ’ 

f = (1 + e )<f" + \Iff"-ef")+b + n 

xm ^ ax' v x Y a y a z' ax 'ax ’ 

f = ( 1 + s )(-b i' + f'+<p t') + b + n , 
ym v ay' ,v Y a x y y a z' ay ‘ay ’ 

f 85 (Its )(0 f'iffl f' + f') + b + tj . (A. 12) 

zm v az A a x Y a y z' az 'az v 

The airspeed measurement V is subject to scale factor 
error e v , bias b , and noise tj v , as 


V = (1 + s )V + b +n 
am v v' a v *v 


(A. 13) 


The barometric altimeter measurement is similarly affected by 
scale factor error e, , bias b, , and noise rj. 


b m = (1 + e h )b b + b h + tj 


m 


'h 


(A. 14) 


Estimate mean and standard deviation.- Let Acp_ be the 
n 

difference between the estimated roll angle cp and the 

measured roll angle 9 at the nth sample point of a sequence 

of m points Then, the mean difference in 9 is 
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1 

m 


(A. 15) 


A<p = E{Acp} 


ra 
2 A<p 
n=l 


n 


The sample variance about the mean is 


2 A . 2, 
a A(p = E{A<p } = 


1 

m~l 


m 

2 (Acp -Acp) 
n=l n 


(A. 16) 


This is computed by storing the residuals Acp^ as Acp is 

computed m Eq. (A. 15). Then, Eq. (A. 16) is computed on a 
second pass through the data. 

A ^ Similar means and variances are computed tor 9, p, q, 
r, V , and h. These do not represent absolute errors in the 

cL 

estimates. Bather, they represent the statistical differences 
between the estimated and directly measured quantities. The 
INS and rate gyro measurements are also subject to errors, and 
these errors are included in the statistics. However, because 
the INS and rate gyro measurements are considered to be of 
extremely high quality with regard to flight control applica- 
tions, they serve as a reasonable standard with which to assess 
the estimators. 
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APPENDIX B 


METHOD NO. 2 STATE ESTIMATOR SOFTWARE 


Chapter IV presents preliminary estimates of the require- 
ments to mechanize the Method No. 2 state estimator on a 
typical microcomputer in terms of memory and run times. To 
make these estimates required definition of specific estimator 
software and coding on a PDP 11 computer. This appendix pre- 
sents the Method 2 estimator code in FORTRAN form. Input 
variables, program constants, and initial computations are 
also given. Then, a listing of the main estimator cycle 
computations in C language (UNIX system) for the PDP 11 is 
presented. 


Method No. 2 Mechanization 

Computer arithmatic, logic, and function requirements . - 


+ 

add 

- 

subtract 

* 

multiply 

/ 

divide 

SQRT 

square root 

SIN 

sine 

C0S 

cosine 

ARSIN 

arc sine 

ARCpS 

arc cosine 

ABS 

absolute value 

• GT. 

greater than 

.LT. 

less than 

.AND. 

logical AND 

IF, THEN 
G0 T 0 

labeled G$ T0 

Definition of 

constants and program variables 

Const ants 

used throughout program' 


Correction terms for linear 
accelerometer 

RKUT - Airspeed smoothing gam 
HL( 1) ,HL(2) ,HL(3) ) 

S0H(1) ,S0H(2) ,S0H(3) f Table quantities to com- 

SL(1),SL(2),SL(3) ( pute Q 

C0N1 > 


0PXC,0PYC,0PZC ) 
BAXC , BAYC , BAZC [ 
SAMR, TAME, FAME ) 
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Constants used throughout program (Coat'd)- 


Correction terms 
for magnetometer 


S ALR , TALR , FALR - Correction terms for angular 

accelerometer 

OCBX, OCBY, OCBZ 
BCBX , BCBY , BCBZ 

SG-XR-, TCXR , SGY-R , FGYR , TCZR , FCZR 
ALR0 , ALR1 , ACN - (ACN function of aircraft mass) 
BX0,BZ0 - (Function of local magnetic field) 

DT , DTT , DTQ - At, At z /2, At 3 /6 
TPI,TP0T,P0T - 2 tc , 3tc/2, te/2 

H0,G - (H0 function of local barometric pressure) 
RKBH , RK1 , RK2 

RKBP , RK3 , RK4 

RKBQ , RK5 , RK6 

RKBR , RK7 , RK8 


Primary filter gams 


Variables initialized to zero or known values: 

BHHN,HHD - Altitude bias and rate 

BHPN,PHN,FIHN - Roll accelerometer bias, roll 

rate, and angle 

BHQN , QHN , ALHN - Pitch accelerometer bias and 

pitch rate, and angle~of-attack 

BHRN,RHN - Yaw accelerometer bias and yaw 

rate 

VAH - True airspeed 

Variables initialized by direct reading or input* 


HHN-HM 

D1j2 

RM 

T 


- Altitude 

- Local magnetic field dip angle 

- Aircraft average mass 

- Time 


Variables initialized by computation: 


BX0=C0S(DL2) ) Local North and down components 
BZ0= s SIN(DL2) 1 of magnetic field 
ACN=RM/ ( CZAL*SW) - (CZAL: stored lift coeffi- 
cient, SW : reference wing 
area) 


Measured (sampled) input variables: 


FXM , FYM , FZM 
VM 

PDM , QDM , RDM 
BXM,BYM,BZM 
HM 

FLPR 


linear accelerometer 
true airspeed 
angular accelerometer 
magnetic field 
barometric altitude 
flap angle 
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Initial computation to cfotain initial yaw angle (on 
groundT . - ~ 

(X) Measure BXM , BYM from magnetometer 

(2) Use equations (given later) to remove magnetometer 
errors and normalize BXM, BYM to BUX,BUY. 

(3) Compute: 

BMB® 1 . / SQRT ( BUX*BUX+BUY*BUY ) 

CSI=BUX*BMB 

SSI=-BUY*BMB 

SIHN=ARC0S(CSI) 

IF(SSI.LT. 0. )SIHN=TPI-SIHN. 

Computations of main estimator equations (cyclic) . - 

(1) Read sampled input variables from buffers . 

(2) Modification of sensor readings . 

Remove linear accelerometer scale factor error, bias, 
and misalignment: 

FXM=0PXC* FXM+BAXC 

FYM=0PYC*FYM+BAYC 

FZM=0PZC*FAM+BAZC 

FXN=FXM+SAMR*FYM-TAMR*FZM 

FYN=-SAMR*FXM+FYM+FAMR*FZM 

FZN=TAMR*FXM-FAMR*FYM+FZM 


Smooth true airspeed and compute § 

VAH- V AH+RKUT * ( VM-VAH ) 

1=1 

IF(HNN,GT.HL(2) )I=2 
IFCHHN.GT.HLC3)) 1=3 
SIG=SL(I)+S0H(I)*(HHN-HL(I)) 
QH=C0N1*S I G*VAH*VAH 


Remove angular accelerometer misalignment : 

PDN=PDM+S ALR * QDM- TALR* RDM 
QDN= - S ALR*PDM+QDM+FALR* RDM 
HDN=TALR*PDM-FALR*QDM+RDM 
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Remove magnetometer scale factor error, bias, and 
misalignment. Normalize. 


BXM=0CBX*BXM+BCBX 
BYM=0CBY*BYM+BCBY 
BZM=0CBZ *BZM+BCBZ 
BXN=BXM+SCXR*BYM+TCXR*BZM 
~-BYN=BYM+SCYR*BXM+FCYR*BZM 
BZN=BZM+TCZR*BXM+FCZR*BYM 
0BMAG=1 . / S QRT ( BXN* BXN+B YN *B YN +B ZN* B ZN ) 
BUX=BXN*0BMAG 
BUY=BYN*0BMAG 
BUZ=BZN*0BMAG 


(3) Compute independent angles . 

A'ngle-of-attack, flight path angle, and pitch angle’ 

ALZR=ALR0+ALR1 *FLPR 
AMN= ALZR- ACN*FZN / QH 
SGAM=HHD/YAH 
GAMP=ARS IN ( SGAM ) 

THHN=GAMP+ALEN*C0S ( FIHN ) 


Roll and yaw angles- 

STHH=SIN(THHN) 

CTHH=C0S(THHN) 

CS IM= ( BUX+BZ0*STHH ) / ( BXp*CTHB ) 

TA= ( BUZ*BUZ+BUY*BUY ) *CTHH* CTHH 
TB=— 2 . * (BZ0+BUX*STHH) *BUZ*CTHH 
TC1= ( BZ0+BUX*STHH ) * ( BZ0+BUX* STHH ) 

TC=TC 1 -BUY *BUY * CTHH* CTHH 
DIS=TB*TB-4 . *TA*TC 
DISR=SQRT(DIS) 

CFMI=0 . 5*(-TB+DISR) /TA 
CFM2=0.5*(-TB-DISR)/TA 

SFM1=BUY* (CFM1+CFM1-1 . ) / (BUZ * CFMl-B Z0 + CTHH 
-BX0*STHH*CSIM) 

SFM2=BUY* ( CFM2 * CFM2 - 1 . ) / (BUZ*CFM2-BZ0*CTHH 
-BX0*STHH*CSIM) 

FMPLS=FIM+PHN*DT 

SNM0=SIN(FMPLS) 

DSF1=ABS( SFMl-SNM^) 

DSF2=ABS ( SFM2-SNM0 ) 

IF(DSF2 LT.DSF1) G0 T0 10 
CFM=CFM1 
SFM=SFM1 
G0 T (p 20 
10 CONTINUE 
CFM=CFM2 
SFM=SFM2 


78 



Roll and yaw angles (Coat'd): 

20 CONTINUE 

FIM=ARSIN( SFM) 

S S IM= ( SFM-BUY* CFM/ BUZ ) *BUZ / BX0 
SIM=ARC0S(CSIM) 

IFCSSIM.LT. 0. )SIM=TPI-SIM 

(4) Primary filter . 

Preliminary computations: 

IF ( ( S IM . GT . TP0T ) . AND . (SIHN.LT. P0T ) ) S IHN=S IHN+TPI 

IF((SIM.LT. P0T ) . AND . C S IHN . GT . TP0T ) ) SIHN=S IHN-TPI 

RSI=SIM-SIHN 

RFI=FIM-FIHN 

RAL=AMN-ALHN 

RH=HM+H0-HHN 

SFIH=SIN(FIHN) 

CFIH=C0S(FIHN) 

TTHH=STHH/CTHH 

C0=RKBH*RH 

C1=FXN*STHH-FYN*SFIH*CTHH-FZN*CFIH*CTHH~G 
+RK1*RH+BHHN 
C2=RK2*RH 
CBP=RKBP*RFI 
C 3 =PDN+ RK3 * RF I +BHPN 
C4=RK4*RFI 
CCP=RKBQ*RAL 
C5=QDN+RK5*RAL+BHQN 
C6= S RK6*RAL+(FZN+G*CFIH*CTHH) /VAH 
CDP=RKBR*CFIH*CTHH*RSI 
C7 = RDN +RK7 * CF IH* CTHH*RS I +BHRN 
C8=RK8*RSI 


Altitude filter 

BHHP=BHHN+C0*DT 

HHDP=HHD+C1*DT+C0*DTT 

HHP=HHN+ ( HHD+C2 ) *DT+C1*DTT+C0*DTQ 

Roll filter* 

BHPP=BHPN + CBP * DT 
PHNP=PHN+C 3 *DT+CBP*DTT 

FIHP=FIHN+ C PHN+C QHN*SFIH+RHN*CF IH ) *TTHH+CR ) *DT 
+ (C3+(C5WIH+C7*CFIH)*TTHH)*DTT 
+ ( CBP+ ( CCP*SF IH+CDP*CFIH ) *TTHH)DTQ 
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Angle-of -attack filter: 


BHQP=BHQN+CCP*DT 

QHNP=QHN+C 5 *DT+CCP*DTT 

ALHP=ALHN+( QHN+C6 ) *DT+C5*DTT+CCP*DTQ 

- Yaw filter * 

BHRP=BHRN+CDP*DT 

RHNP=RHN+C7*DT+CDP*DTT 

S IHP=S IHN+C8 *DT+ ( ( RHN*CEIH+QHN*SEIH ) *DT 
+(C7*CPIH+C5*SFIH)*DTT) /CTBH 
+( CDP * CE IH+CCP *SF I H ) *DTQ/CTHH 

Time update: 

T=T+DT 

BHHN=BHHP 

HHD-HHDP 

HHN=HHP 

BHPN=BEPP 

BHQN=BHQP 

BHRN=BHRP 

PHN=PHNP 

QHN=QHNP 

RHN=RHNP 

EIHN=FIHP 

ALHN=ALHP 

SIHN=SIHP 

(5) Output estimates (display and/or control computations). 


(6) Cycle back to read new samples . 


Main Estimator Code in UNIX C Language 
main ( ) { 

float fxm, £ym,£zm,opxc,opyc,opzc,oaxc,bayc,bazc,r)cbh, 
fxn,£yn,fzn,sae"r , ta-nr ,tamr , van,rKut,v'n,hhn, 
hli3],slC3i,soh[3J , siq ,qh ,conl ,pdn ,qdn , rdn, 
pdm , qdm, ran, sal r , td Lr , fair , bxm ,pym ,bzm , 
ocbx , ocbv , ocoz, oc by , r cuy , ncbz , bxn , byn , bzn , 
scxr , scyr , tcxr,tczr,fcvr,fczr,obmaq, 
bux,buy,buz,aizr,alro,alrl,f lpr ,an.n ,acn, 
saam ,hhd , qanp , tnnn , al hn, f inn , stnh , cthh , 
bxo,ozo,csittfta,tn,tc,dis,disr,cfmi,cfni2, 
s£mi,s£ir2,tmDis,pnn,ot,snno,dsfl,dsf2, 
cfm,sf f r>,£i'i,ssin,si~>,tpi,tpot,pot,slhn,rsif 
rfi/ral,rh»nn- , no f nnn,s£ib,ctih/ttrih,cO,cl ,g, 
rXi , bnhn,c2,rk2 , ctr>r> , rkfc p , c 3 , rK3,bbpn, 
c4,rX4,ccp,rXba,c5,r l <5 / bhqn f c6,rK6,cdp, 
rtcbr ,c7,rk7 ,ohrn,ce , r<8 ,bnno , hhdp , dtt , 
hbp , a t q , onop , pbnc , f ihp , ahn , r nn , bhqo , qhnn , 
alhp t bnrp , rnno, sinp ,t? 
int i; 
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read: 

fxm = ODXC*fxm * n-iYC‘, 

fy.n = opyc + £\' 1 '. + -,ovcj 

fzm s ooz(**tzr l + haze; 

fxn = £ x Hi + safi.r*tv n - tamr*fzm; 

fyn = -saTir^tx" f fy.it f famr*fzm; 

fzn = t amr+fxr - fdnr*fyr n + fzm; 

van = van i rknr * { vw - van); 

1 = nt 

it Crum > n i £ 1 J ) l = \ ; 
it (ntm > aii2n t = 2; 
si-? s sltii + sn«-jf f j*(nnn ~ nifij); 

Oh s coni * sia Moo* (van, 2 ));' 

pnn = ofii + S4ir*irt*n - cair*ra f n; 

qriA. s -&rtir + pd'i + t fiir + rnm; 

rdn s talr *rvj.i - lalr+od" 1 + ram? 

bxc 3 ncoxH-X 1 '! * ''C-m; 

by m = ocby*oyi> + 

bzn = ocDZ*bzm + henz; 

bxn s oxn + scxr*bvp + tcxr*bzm; 

byn = by i + scyr*cxin + f-cyr*dzm; 

bzn = bzn + tczr*n<r; + fczr*by">; 

oornaa = 1 /% jr t (oxr *oxr, t byo*nyn + bzn * bzn)? 

bU'X = hxn *ohman? 

buy = oyn-*or)P3'j ; 

buz = bzn*ODf;M > ; 


alzr * alro + alri+flcr? 

amn s alzr - acn^f zn/an; 

saaui s hrvJ/vah? 

game = ar s In C soa'i. ) ; 

tbhn = qan.^ + alnn+cos C f inn) ; 

sehn = sin(tnnn)? 

ctnh = cos( tblm ) ; 

csim = (box + bzn-f stbh) / Coxo*cthb ) ; 

ta = ( buz t buz + cuy * buy ) * C cthn * cthh); 

tb = -?,*(bzo + ^..}x*sthn)*buz*cthh; 

tc = powCbzo + oux * sthh,2) -pcv'ibuy * cthh , 2 ); 

dis = tb*tn - 4*ta*tc? 

disr 3 sqrtCais); 

cfml = 0.5*(-tb + disr)/tn; 

cf'H2 = O.S’H-tb - disr)/ta; 

sfni = buy*Cc£ml * etui - l . ) / (buz*cf ml - bzo*ctnh - bxo*sthh*csim) ; 

s f m 2 = buy* ( cf T‘? *c f m2 - 3 . ) / (buz*cf m2 - bzo*cthh - bxo*sthh*csim); 

fmpls = fim + cbn*ot; 

sn no = sinCf upl s ) ; 

dsfl 3 absCstm) - snmo); 

dsf2 = abs(sf(n2 - snuo); 

Lf(dsf2 < ostl) noth ten; 

cfm = ctm] ; 

sfm = sfnl; 

goto tfcnty; 

ten: ctm = cfm?: 

s f m = s f m 2 ; 


DSIGINAC PAG® SI 
QE POOR QUALTSX 
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twnty: fim = arsin Csftn) ; 

ssim a Csf'n - 5 jy *cf m/buz) *buz/bxo ; 

sim a arcosCcsn.i; 

if tssim < 0 ) sit) = tpi * sim; 

if (CsiT>tpot) Csihn < potiJ sihn = sihn + tDi; 
if C(sim<pot) isihn > tpot)) sihn = sihn - tpi; 


rsi 

a sim 

- sinn; 

-r-fi 

= f im 

= f i-h n ; 

ral 

= amn 

- al’-o; 

rh = 

: hm 4 

no - ft si n ; 


s f 1 h = sinCfionjr 
cfih a cos (t inn); 
tthn = sthh/ctm; 
cO = rkbh*rn; 

cl = fxn*stnh - f yn*sf ih*cthh - f zn+cf in+cthn - q 
+ rkl*rh + onhn; 
c2 * rk2*rh; 
cbp = rkDp*rfi; 


c3 = pdn + rk3*rfi + ohpn; 

c4 = rk4*r£i; 

cco = rkbq*ral; 

c5 = adn + rx.5*ral + ehqn? 

c6 = rkfi>*ral + Cfzn + q*cf ih*cthh) /vah; 

cdp = rkor*cf ih*ctnn*rsi ; 

cl = rdn + rjc7*cf ih*cthh*rsi + bhrn; 

c8 = rk8*rsi; 

bhhp = thhn 4 cO*dt; 

hhao a nnd + ci*dt ♦ cO*ctt; 

hhp = hhn + (hho + c2)*dt + ci*dtt 4 cO*dtq; 

bhpp = bhpn + cor-*dt; 

phnp = phn + c3*dt + cbp*att; 

fine = fihn + ("hn + Cqhn*sfih + rhn*cf ih) *tthh + c4)#dt 
+ (c3 + CcS*sfin + c7*cf in) *tthh) *att 
+ (cop + (cco*sfib + cdp*cfih)*tthnD*dtq; 
bbqp = bmn + ccr*at; 
qhno = qhn + c5*Jt + cco*Jtt; 

albp = aim + Conn + c6)*qt + c5*ott + ccp*dtq; 

bhrp = bnrn + cop*dt; 

rbnp = ran + c?*it + cop*dtt; 

sihp = sinn + c8*at 4 C(rhn*cfih + qhn*sfih)*dt 
+ (c7*c£in + c5*sf ih) *dtt ) /cthh 
4 (cdD *c£in ■» ccn^sfih) *dtq/cthh; 
t = t 4 dt; 
bhhn = ohno; 
hbn = hhap; 
hhn = hnp; 
bbpn = bnpp; 
bban = bhqp; 
bhrn = onn? 
phn = phno; 
qhn = qhnp; 
rhn a rhnp f 
finn = finp; 
alhn = alhp? 
sihn a sihp'; 
goto read; 
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